//////////////////////////////////////////////////////////////////////
//
//	ZETeam source code 
//	Copyright (c) ZETeam 2011-..
//	
//	File: CMatrix.h
//  Description: matrix classes. Based on Id Software source code.
// 
//	History:
//	- February 27, 2011: Created by Max Streltsov
//
//////////////////////////////////////////////////////////////////////

#ifndef MATRIX_H
#define MATRIX_H

#include "global_defs.h"

#include "CMath.h"
#include "CVector.h"

/*
===============================================================================

  Matrix classes, all matrices are row-major except CMat3

===============================================================================
*/

#define MATRIX_INVERSE_EPSILON		1e-14
#define MATRIX_EPSILON				1e-6

class CAngles;
class CQuat;
class CCQuat;
class CRotation;
class CMat4;

//===============================================================
//
//	CMat2 - 2x2 matrix
//
//===============================================================

class CMat2
{
public:
					CMat2( void );
					explicit CMat2( const CVec2 &x, const CVec2 &y );
					explicit CMat2( const float xx, const float xy, const float yx, const float yy );
					explicit CMat2( const float src[ 2 ][ 2 ] );

	const CVec2 &	operator[]( int index ) const;
	CVec2 &		operator[]( int index );
	CMat2			operator-() const;
	CMat2			operator*( const float a ) const;
	CVec2			operator*( const CVec2 &vec ) const;
	CMat2			operator*( const CMat2 &a ) const;
	CMat2			operator+( const CMat2 &a ) const;
	CMat2			operator-( const CMat2 &a ) const;
	CMat2 &		operator*=( const float a );
	CMat2 &		operator*=( const CMat2 &a );
	CMat2 &		operator+=( const CMat2 &a );
	CMat2 &		operator-=( const CMat2 &a );

	friend CMat2	operator*( const float a, const CMat2 &mat );
	friend CVec2	operator*( const CVec2 &vec, const CMat2 &mat );
	friend CVec2 &	operator*=( CVec2 &vec, const CMat2 &mat );

	bool			compare( const CMat2 &a ) const;						// exact compare, no epsilon
	bool			compare( const CMat2 &a, const float epsilon ) const;	// compare with epsilon
	bool			operator==( const CMat2 &a ) const;					// exact compare, no epsilon
	bool			operator!=( const CMat2 &a ) const;					// exact compare, no epsilon

	void			zero( void );
	void			identity( void );
	bool			isidentity( const float epsilon = MATRIX_EPSILON ) const;
	bool			isSymmetric( const float epsilon = MATRIX_EPSILON ) const;
	bool			isDiagonal( const float epsilon = MATRIX_EPSILON ) const;

	float			trace( void ) const;
	float			determinant( void ) const;
	CMat2			transpose( void ) const;	// returns transpose
	CMat2 &		transposeSelf( void );
	CMat2			inverse( void ) const;		// returns the inverse ( m * m.inverse() = identity )
	bool			inverseSelf( void );		// returns false if determinant is zero
	CMat2			inverseFast( void ) const;	// returns the inverse ( m * m.inverse() = identity )
	bool			inverseFastSelf( void );	// returns false if determinant is zero

	int				getDimension( void ) const;

	const float *	toFloatPtr( void ) const;
	float *			toFloatPtr( void );
	//const char *	toString( int precision = 2 ) const;

private:
	CVec2			mat[ 2 ];
};

extern CMat2 mat2_zero;
extern CMat2 mat2_identity;
#define mat2_default	mat2_identity

Z_INLINE CMat2::CMat2( void ) {
}

Z_INLINE CMat2::CMat2( const CVec2 &x, const CVec2 &y ) {
	mat[ 0 ].x = x.x; mat[ 0 ].y = x.y;
	mat[ 1 ].x = y.x; mat[ 1 ].y = y.y;
}

Z_INLINE CMat2::CMat2( const float xx, const float xy, const float yx, const float yy ) {
	mat[ 0 ].x = xx; mat[ 0 ].y = xy;
	mat[ 1 ].x = yx; mat[ 1 ].y = yy;
}

Z_INLINE CMat2::CMat2( const float src[ 2 ][ 2 ] ) {
	memcpy( mat, src, 2 * 2 * sizeof( float ) );
}

Z_INLINE const CVec2 &CMat2::operator[]( int index ) const {
	//assert( ( index >= 0 ) && ( index < 2 ) );
	return mat[ index ];
}

Z_INLINE CVec2 &CMat2::operator[]( int index ) {
	//assert( ( index >= 0 ) && ( index < 2 ) );
	return mat[ index ];
}

Z_INLINE CMat2 CMat2::operator-() const {
	return CMat2(	-mat[0][0], -mat[0][1],
					-mat[1][0], -mat[1][1] );
}

Z_INLINE CVec2 CMat2::operator*( const CVec2 &vec ) const {
	return CVec2(
		mat[ 0 ].x * vec.x + mat[ 0 ].y * vec.y,
		mat[ 1 ].x * vec.x + mat[ 1 ].y * vec.y );
}

Z_INLINE CMat2 CMat2::operator*( const CMat2 &a ) const {
	return CMat2(
		mat[0].x * a[0].x + mat[0].y * a[1].x,
		mat[0].x * a[0].y + mat[0].y * a[1].y,
		mat[1].x * a[0].x + mat[1].y * a[1].x,
		mat[1].x * a[0].y + mat[1].y * a[1].y );
}

Z_INLINE CMat2 CMat2::operator*( const float a ) const {
	return CMat2(
		mat[0].x * a, mat[0].y * a, 
		mat[1].x * a, mat[1].y * a );
}

Z_INLINE CMat2 CMat2::operator+( const CMat2 &a ) const {
	return CMat2(
		mat[0].x + a[0].x, mat[0].y + a[0].y, 
		mat[1].x + a[1].x, mat[1].y + a[1].y );
}
    
Z_INLINE CMat2 CMat2::operator-( const CMat2 &a ) const {
	return CMat2(
		mat[0].x - a[0].x, mat[0].y - a[0].y,
		mat[1].x - a[1].x, mat[1].y - a[1].y );
}

Z_INLINE CMat2 &CMat2::operator*=( const float a ) {
	mat[0].x *= a; mat[0].y *= a;
	mat[1].x *= a; mat[1].y *= a;

    return *this;
}

Z_INLINE CMat2 &CMat2::operator*=( const CMat2 &a ) {
	float x, y;
	x = mat[0].x; y = mat[0].y;
	mat[0].x = x * a[0].x + y * a[1].x;
	mat[0].y = x * a[0].y + y * a[1].y;
	x = mat[1].x; y = mat[1].y;
	mat[1].x = x * a[0].x + y * a[1].x;
	mat[1].y = x * a[0].y + y * a[1].y;
	return *this;
}

Z_INLINE CMat2 &CMat2::operator+=( const CMat2 &a ) {
	mat[0].x += a[0].x; mat[0].y += a[0].y;
	mat[1].x += a[1].x; mat[1].y += a[1].y;

    return *this;
}

Z_INLINE CMat2 &CMat2::operator-=( const CMat2 &a ) {
	mat[0].x -= a[0].x; mat[0].y -= a[0].y;
	mat[1].x -= a[1].x; mat[1].y -= a[1].y;

    return *this;
}

Z_INLINE CVec2 operator*( const CVec2 &vec, const CMat2 &mat ) {
	return mat * vec;
}

Z_INLINE CMat2 operator*( const float a, CMat2 const &mat ) {
	return mat * a;
}

Z_INLINE CVec2 &operator*=( CVec2 &vec, const CMat2 &mat ) {
	vec = mat * vec;
	return vec;
}

Z_INLINE bool CMat2::compare( const CMat2 &a ) const {
	if ( mat[0].compare( a[0] ) &&
		mat[1].compare( a[1] ) ) {
		return true;
	}
	return false;
}

Z_INLINE bool CMat2::compare( const CMat2 &a, const float epsilon ) const {
	if ( mat[0].compare( a[0], epsilon ) &&
		mat[1].compare( a[1], epsilon ) ) {
		return true;
	}
	return false;
}

Z_INLINE bool CMat2::operator==( const CMat2 &a ) const {
	return compare( a );
}

Z_INLINE bool CMat2::operator!=( const CMat2 &a ) const {
	return !compare( a );
}

Z_INLINE void CMat2::zero( void ) {
	mat[0].zero();
	mat[1].zero();
}

Z_INLINE void CMat2::identity( void ) {
	*this = mat2_identity;
}

Z_INLINE bool CMat2::isidentity( const float epsilon ) const {
	return compare( mat2_identity, epsilon );
}

Z_INLINE bool CMat2::isSymmetric( const float epsilon ) const {
	return ( CZMath::Fabs( mat[0][1] - mat[1][0] ) < epsilon );
}

Z_INLINE bool CMat2::isDiagonal( const float epsilon ) const {
	if ( CZMath::Fabs( mat[0][1] ) > epsilon ||
		CZMath::Fabs( mat[1][0] ) > epsilon ) {
		return false;
	}
	return true;
}

Z_INLINE float CMat2::trace( void ) const {
	return ( mat[0][0] + mat[1][1] );
}

Z_INLINE float CMat2::determinant( void ) const {
	return mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];
}

Z_INLINE CMat2 CMat2::transpose( void ) const {
	return CMat2(	mat[0][0], mat[1][0],
					mat[0][1], mat[1][1] );
}

Z_INLINE CMat2 &CMat2::transposeSelf( void ) {
	float tmp;

	tmp = mat[0][1];
	mat[0][1] = mat[1][0];
	mat[1][0] = tmp;

	return *this;
}

Z_INLINE CMat2 CMat2::inverse( void ) const {
	CMat2 invMat;

	invMat = *this;
	int r = invMat.inverseSelf();
	assert( r );
	return invMat;
}

Z_INLINE CMat2 CMat2::inverseFast( void ) const {
	CMat2 invMat;

	invMat = *this;
	int r = invMat.inverseFastSelf();
	assert( r );
	return invMat;
}

Z_INLINE int CMat2::getDimension( void ) const {
	return 4;
}

Z_INLINE const float *CMat2::toFloatPtr( void ) const {
	return mat[0].toFloatPtr();
}

Z_INLINE float *CMat2::toFloatPtr( void ) {
	return mat[0].toFloatPtr();
}


//===============================================================
//
//	CMat3 - 3x3 matrix
//
//	NOTE:	matrix is column-major
//
//===============================================================

class CMat3 {
public:
					CMat3( void );
					explicit CMat3( const CVec3 &x, const CVec3 &y, const CVec3 &z );
					explicit CMat3( const float xx, const float xy, const float xz, const float yx, const float yy, const float yz, const float zx, const float zy, const float zz );
					explicit CMat3( const float src[ 3 ][ 3 ] );

	const CVec3 &	operator[]( int index ) const;
	CVec3 &		operator[]( int index );
	CMat3			operator-() const;
	CMat3			operator*( const float a ) const;
	CVec3			operator*( const CVec3 &vec ) const;
	CMat3			operator*( const CMat3 &a ) const;
	CMat3			operator+( const CMat3 &a ) const;
	CMat3			operator-( const CMat3 &a ) const;
	CMat3 &		operator*=( const float a );
	CMat3 &		operator*=( const CMat3 &a );
	CMat3 &		operator+=( const CMat3 &a );
	CMat3 &		operator-=( const CMat3 &a );

	friend CMat3	operator*( const float a, const CMat3 &mat );
	friend CVec3	operator*( const CVec3 &vec, const CMat3 &mat );
	friend CVec3 &	operator*=( CVec3 &vec, const CMat3 &mat );

	bool			compare( const CMat3 &a ) const;						// exact compare, no epsilon
	bool			compare( const CMat3 &a, const float epsilon ) const;	// compare with epsilon
	bool			operator==( const CMat3 &a ) const;					// exact compare, no epsilon
	bool			operator!=( const CMat3 &a ) const;					// exact compare, no epsilon

	void			zero( void );
	void			identity( void );
	bool			isidentity( const float epsilon = MATRIX_EPSILON ) const;
	bool			isSymmetric( const float epsilon = MATRIX_EPSILON ) const;
	bool			isDiagonal( const float epsilon = MATRIX_EPSILON ) const;
	bool			isRotated( void ) const;

	void			projectVector( const CVec3 &src, CVec3 &dst ) const;
	void			unprojectVector( const CVec3 &src, CVec3 &dst ) const;

	bool			fixDegeneracies( void );	// fix degenerate axial cases
	bool			fixDenormals( void );		// change tiny numbers to zero

	float			trace( void ) const;
	float			determinant( void ) const;
	CMat3			orthoNormalize( void ) const;
	CMat3 &		orthoNormalizeSelf( void );
	CMat3			transpose( void ) const;	// returns transpose
	CMat3 &		transposeSelf( void );
	CMat3			inverse( void ) const;		// returns the inverse ( m * m.inverse() = identity )
	bool			inverseSelf( void );		// returns false if determinant is zero
	CMat3			inverseFast( void ) const;	// returns the inverse ( m * m.inverse() = identity )
	bool			inverseFastSelf( void );	// returns false if determinant is zero
	CMat3			transposeMultiply( const CMat3 &b ) const;

	CMat3			inertiaTranslate( const float mass, const CVec3 &centerOfMass, const CVec3 &translation ) const;
	CMat3 &		inertiaTranslateSelf( const float mass, const CVec3 &centerOfMass, const CVec3 &translation );
	CMat3			inertiaRotate( const CMat3 &rotation ) const;
	CMat3 &		inertiaRotateSelf( const CMat3 &rotation );

	int				getDimension( void ) const;

	CAngles		toAngles( void ) const;
	CQuat			toQuat( void ) const;
	CCQuat			toCQuat( void ) const;
	CRotation		toRotation( void ) const;
	CMat4			toMat4( void ) const;
	CVec3			toAngularVelocity( void ) const;
	const float *	toFloatPtr( void ) const;
	float *			toFloatPtr( void );
	//const char *	toString( int precision = 2 ) const;

	friend void		transposeMultiply( const CMat3 &inv, const CMat3 &b, CMat3 &dst );
	friend CMat3	skewSymmetric( CVec3 const &src );

private:
	CVec3			mat[ 3 ];
};

extern CMat3 mat3_zero;
extern CMat3 mat3_identity;
#define mat3_default	mat3_identity

Z_INLINE CMat3::CMat3( void ) {
}

Z_INLINE CMat3::CMat3( const CVec3 &x, const CVec3 &y, const CVec3 &z ) {
	mat[ 0 ].x = x.x; mat[ 0 ].y = x.y; mat[ 0 ].z = x.z;
	mat[ 1 ].x = y.x; mat[ 1 ].y = y.y; mat[ 1 ].z = y.z;
	mat[ 2 ].x = z.x; mat[ 2 ].y = z.y; mat[ 2 ].z = z.z;
}

Z_INLINE CMat3::CMat3( const float xx, const float xy, const float xz, const float yx, const float yy, const float yz, const float zx, const float zy, const float zz ) {
	mat[ 0 ].x = xx; mat[ 0 ].y = xy; mat[ 0 ].z = xz;
	mat[ 1 ].x = yx; mat[ 1 ].y = yy; mat[ 1 ].z = yz;
	mat[ 2 ].x = zx; mat[ 2 ].y = zy; mat[ 2 ].z = zz;
}

Z_INLINE CMat3::CMat3( const float src[ 3 ][ 3 ] ) {
	memcpy( mat, src, 3 * 3 * sizeof( float ) );
}

Z_INLINE const CVec3 &CMat3::operator[]( int index ) const {
	//assert( ( index >= 0 ) && ( index < 3 ) );
	return mat[ index ];
}

Z_INLINE CVec3 &CMat3::operator[]( int index ) {
	//assert( ( index >= 0 ) && ( index < 3 ) );
	return mat[ index ];
}

Z_INLINE CMat3 CMat3::operator-() const {
	return CMat3(	-mat[0][0], -mat[0][1], -mat[0][2],
					-mat[1][0], -mat[1][1], -mat[1][2],
					-mat[2][0], -mat[2][1], -mat[2][2] );
}

Z_INLINE CVec3 CMat3::operator*( const CVec3 &vec ) const {
	return CVec3(
		mat[ 0 ].x * vec.x + mat[ 1 ].x * vec.y + mat[ 2 ].x * vec.z,
		mat[ 0 ].y * vec.x + mat[ 1 ].y * vec.y + mat[ 2 ].y * vec.z,
		mat[ 0 ].z * vec.x + mat[ 1 ].z * vec.y + mat[ 2 ].z * vec.z );
}

Z_INLINE CMat3 CMat3::operator*( const CMat3 &a ) const {
	int i, j;
	const float *m1Ptr, *m2Ptr;
	float *dstPtr;
	CMat3 dst;

	m1Ptr = reinterpret_cast<const float *>(this);
	m2Ptr = reinterpret_cast<const float *>(&a);
	dstPtr = reinterpret_cast<float *>(&dst);

	for ( i = 0; i < 3; i++ ) {
		for ( j = 0; j < 3; j++ ) {
			*dstPtr = m1Ptr[0] * m2Ptr[ 0 * 3 + j ]
					+ m1Ptr[1] * m2Ptr[ 1 * 3 + j ]
					+ m1Ptr[2] * m2Ptr[ 2 * 3 + j ];
			dstPtr++;
		}
		m1Ptr += 3;
	}
	return dst;
}

Z_INLINE CMat3 CMat3::operator*( const float a ) const {
	return CMat3(
		mat[0].x * a, mat[0].y * a, mat[0].z * a,
		mat[1].x * a, mat[1].y * a, mat[1].z * a,
		mat[2].x * a, mat[2].y * a, mat[2].z * a );
}

Z_INLINE CMat3 CMat3::operator+( const CMat3 &a ) const {
	return CMat3(
		mat[0].x + a[0].x, mat[0].y + a[0].y, mat[0].z + a[0].z,
		mat[1].x + a[1].x, mat[1].y + a[1].y, mat[1].z + a[1].z,
		mat[2].x + a[2].x, mat[2].y + a[2].y, mat[2].z + a[2].z );
}
    
Z_INLINE CMat3 CMat3::operator-( const CMat3 &a ) const {
	return CMat3(
		mat[0].x - a[0].x, mat[0].y - a[0].y, mat[0].z - a[0].z,
		mat[1].x - a[1].x, mat[1].y - a[1].y, mat[1].z - a[1].z,
		mat[2].x - a[2].x, mat[2].y - a[2].y, mat[2].z - a[2].z );
}

Z_INLINE CMat3 &CMat3::operator*=( const float a ) {
	mat[0].x *= a; mat[0].y *= a; mat[0].z *= a;
	mat[1].x *= a; mat[1].y *= a; mat[1].z *= a; 
	mat[2].x *= a; mat[2].y *= a; mat[2].z *= a;

    return *this;
}

Z_INLINE CMat3 &CMat3::operator*=( const CMat3 &a ) {
	int i, j;
	const float *m2Ptr;
	float *m1Ptr, dst[3];

	m1Ptr = reinterpret_cast<float *>(this);
	m2Ptr = reinterpret_cast<const float *>(&a);

	for ( i = 0; i < 3; i++ ) {
		for ( j = 0; j < 3; j++ ) {
			dst[j]  = m1Ptr[0] * m2Ptr[ 0 * 3 + j ]
					+ m1Ptr[1] * m2Ptr[ 1 * 3 + j ]
					+ m1Ptr[2] * m2Ptr[ 2 * 3 + j ];
		}
		m1Ptr[0] = dst[0]; m1Ptr[1] = dst[1]; m1Ptr[2] = dst[2];
		m1Ptr += 3;
	}
	return *this;
}

Z_INLINE CMat3 &CMat3::operator+=( const CMat3 &a ) {
	mat[0].x += a[0].x; mat[0].y += a[0].y; mat[0].z += a[0].z;
	mat[1].x += a[1].x; mat[1].y += a[1].y; mat[1].z += a[1].z;
	mat[2].x += a[2].x; mat[2].y += a[2].y; mat[2].z += a[2].z;

    return *this;
}

Z_INLINE CMat3 &CMat3::operator-=( const CMat3 &a ) {
	mat[0].x -= a[0].x; mat[0].y -= a[0].y; mat[0].z -= a[0].z;
	mat[1].x -= a[1].x; mat[1].y -= a[1].y; mat[1].z -= a[1].z;
	mat[2].x -= a[2].x; mat[2].y -= a[2].y; mat[2].z -= a[2].z;

    return *this;
}

Z_INLINE CVec3 operator*( const CVec3 &vec, const CMat3 &mat ) {
	return mat * vec;
}

Z_INLINE CMat3 operator*( const float a, const CMat3 &mat ) {
	return mat * a;
}

Z_INLINE CVec3 &operator*=( CVec3 &vec, const CMat3 &mat ) {
	float x = mat[ 0 ].x * vec.x + mat[ 1 ].x * vec.y + mat[ 2 ].x * vec.z;
	float y = mat[ 0 ].y * vec.x + mat[ 1 ].y * vec.y + mat[ 2 ].y * vec.z;
	vec.z = mat[ 0 ].z * vec.x + mat[ 1 ].z * vec.y + mat[ 2 ].z * vec.z;
	vec.x = x;
	vec.y = y;
	return vec;
}

Z_INLINE bool CMat3::compare( const CMat3 &a ) const {
	if ( mat[0].compare( a[0] ) &&
		mat[1].compare( a[1] ) &&
		mat[2].compare( a[2] ) ) {
		return true;
	}
	return false;
}

Z_INLINE bool CMat3::compare( const CMat3 &a, const float epsilon ) const {
	if ( mat[0].compare( a[0], epsilon ) &&
		mat[1].compare( a[1], epsilon ) &&
		mat[2].compare( a[2], epsilon ) ) {
		return true;
	}
	return false;
}

Z_INLINE bool CMat3::operator==( const CMat3 &a ) const {
	return compare( a );
}

Z_INLINE bool CMat3::operator!=( const CMat3 &a ) const {
	return !compare( a );
}

Z_INLINE void CMat3::zero( void ) {
	memset( mat, 0, sizeof( CMat3 ) );
}

Z_INLINE void CMat3::identity( void ) {
	*this = mat3_identity;
}

Z_INLINE bool CMat3::isidentity( const float epsilon ) const {
	return compare( mat3_identity, epsilon );
}

Z_INLINE bool CMat3::isSymmetric( const float epsilon ) const {
	if ( CZMath::Fabs( mat[0][1] - mat[1][0] ) > epsilon ) {
		return false;
	}
	if ( CZMath::Fabs( mat[0][2] - mat[2][0] ) > epsilon ) {
		return false;
	}
	if ( CZMath::Fabs( mat[1][2] - mat[2][1] ) > epsilon ) {
		return false;
	}
	return true;
}

Z_INLINE bool CMat3::isDiagonal( const float epsilon ) const {
	if ( CZMath::Fabs( mat[0][1] ) > epsilon ||
		CZMath::Fabs( mat[0][2] ) > epsilon ||
		CZMath::Fabs( mat[1][0] ) > epsilon ||
		CZMath::Fabs( mat[1][2] ) > epsilon ||
		CZMath::Fabs( mat[2][0] ) > epsilon ||
		CZMath::Fabs( mat[2][1] ) > epsilon ) {
		return false;
	}
	return true;
}

Z_INLINE bool CMat3::isRotated( void ) const {
	return !compare( mat3_identity );
}

Z_INLINE void CMat3::projectVector( const CVec3 &src, CVec3 &dst ) const {
	dst.x = src * mat[ 0 ];
	dst.y = src * mat[ 1 ];
	dst.z = src * mat[ 2 ];
}

Z_INLINE void CMat3::unprojectVector( const CVec3 &src, CVec3 &dst ) const {
	dst = mat[ 0 ] * src.x + mat[ 1 ] * src.y + mat[ 2 ] * src.z;
}

Z_INLINE bool CMat3::fixDegeneracies( void ) {
	bool r = mat[0].fixDegenerateNormal();
	r |= mat[1].fixDegenerateNormal();
	r |= mat[2].fixDegenerateNormal();
	return r;
}

Z_INLINE bool CMat3::fixDenormals( void ) {
	bool r = mat[0].fixDenormals();
	r |= mat[1].fixDenormals();
	r |= mat[2].fixDenormals();
	return r;
}

Z_INLINE float CMat3::trace( void ) const {
	return ( mat[0][0] + mat[1][1] + mat[2][2] );
}

Z_INLINE CMat3 CMat3::orthoNormalize( void ) const {
	CMat3 ortho;

	ortho = *this;
	ortho[ 0 ].normalize();
	ortho[ 2 ].cross( mat[ 0 ], mat[ 1 ] );
	ortho[ 2 ].normalize();
	ortho[ 1 ].cross( mat[ 2 ], mat[ 0 ] );
	ortho[ 1 ].normalize();
	return ortho;
}

Z_INLINE CMat3 &CMat3::orthoNormalizeSelf( void ) {
	mat[ 0 ].normalize();
	mat[ 2 ].cross( mat[ 0 ], mat[ 1 ] );
	mat[ 2 ].normalize();
	mat[ 1 ].cross( mat[ 2 ], mat[ 0 ] );
	mat[ 1 ].normalize();
	return *this;
}

Z_INLINE CMat3 CMat3::transpose( void ) const {
	return CMat3(	mat[0][0], mat[1][0], mat[2][0],
					mat[0][1], mat[1][1], mat[2][1],
					mat[0][2], mat[1][2], mat[2][2] );
}

Z_INLINE CMat3 &CMat3::transposeSelf( void ) {
	float tmp0, tmp1, tmp2;

	tmp0 = mat[0][1];
	mat[0][1] = mat[1][0];
	mat[1][0] = tmp0;
	tmp1 = mat[0][2];
	mat[0][2] = mat[2][0];
	mat[2][0] = tmp1;
	tmp2 = mat[1][2];
	mat[1][2] = mat[2][1];
	mat[2][1] = tmp2;

	return *this;
}

Z_INLINE CMat3 CMat3::inverse( void ) const {
	CMat3 invMat;

	invMat = *this;
	int r = invMat.inverseSelf();
	assert( r );
	return invMat;
}

Z_INLINE CMat3 CMat3::inverseFast( void ) const {
	CMat3 invMat;

	invMat = *this;
	int r = invMat.inverseFastSelf();
	assert( r );
	return invMat;
}

Z_INLINE CMat3 CMat3::transposeMultiply( const CMat3 &b ) const {
	return CMat3(	mat[0].x * b[0].x + mat[1].x * b[1].x + mat[2].x * b[2].x,
					mat[0].x * b[0].y + mat[1].x * b[1].y + mat[2].x * b[2].y,
					mat[0].x * b[0].z + mat[1].x * b[1].z + mat[2].x * b[2].z,
					mat[0].y * b[0].x + mat[1].y * b[1].x + mat[2].y * b[2].x,
					mat[0].y * b[0].y + mat[1].y * b[1].y + mat[2].y * b[2].y,
					mat[0].y * b[0].z + mat[1].y * b[1].z + mat[2].y * b[2].z,
					mat[0].z * b[0].x + mat[1].z * b[1].x + mat[2].z * b[2].x,
					mat[0].z * b[0].y + mat[1].z * b[1].y + mat[2].z * b[2].y,
					mat[0].z * b[0].z + mat[1].z * b[1].z + mat[2].z * b[2].z );
}

Z_INLINE void transposeMultiply( const CMat3 &transpose, const CMat3 &b, CMat3 &dst ) {
	dst[0].x = transpose[0].x * b[0].x + transpose[1].x * b[1].x + transpose[2].x * b[2].x;
	dst[0].y = transpose[0].x * b[0].y + transpose[1].x * b[1].y + transpose[2].x * b[2].y;
	dst[0].z = transpose[0].x * b[0].z + transpose[1].x * b[1].z + transpose[2].x * b[2].z;
	dst[1].x = transpose[0].y * b[0].x + transpose[1].y * b[1].x + transpose[2].y * b[2].x;
	dst[1].y = transpose[0].y * b[0].y + transpose[1].y * b[1].y + transpose[2].y * b[2].y;
	dst[1].z = transpose[0].y * b[0].z + transpose[1].y * b[1].z + transpose[2].y * b[2].z;
	dst[2].x = transpose[0].z * b[0].x + transpose[1].z * b[1].x + transpose[2].z * b[2].x;
	dst[2].y = transpose[0].z * b[0].y + transpose[1].z * b[1].y + transpose[2].z * b[2].y;
	dst[2].z = transpose[0].z * b[0].z + transpose[1].z * b[1].z + transpose[2].z * b[2].z;
}

Z_INLINE CMat3 skewSymmetric( CVec3 const &src ) {
	return CMat3( 0.0f, -src.z,  src.y, src.z,   0.0f, -src.x, -src.y,  src.x,   0.0f );
}

Z_INLINE int CMat3::getDimension( void ) const {
	return 9;
}

Z_INLINE const float *CMat3::toFloatPtr( void ) const {
	return mat[0].toFloatPtr();
}

Z_INLINE float *CMat3::toFloatPtr( void ) {
	return mat[0].toFloatPtr();
}


//===============================================================
//
//	CMat4 - 4x4 matrix
//
//===============================================================

class CMat4 {
public:
					CMat4( void );
					explicit CMat4( const CVec4 &x, const CVec4 &y, const CVec4 &z, const CVec4 &w );
					explicit CMat4(const float xx, const float xy, const float xz, const float xw,
									const float yx, const float yy, const float yz, const float yw,
									const float zx, const float zy, const float zz, const float zw,
									const float wx, const float wy, const float wz, const float ww );
					explicit CMat4( const CMat3 &rotation, const CVec3 &translation );
					explicit CMat4( const float src[ 4 ][ 4 ] );

	const CVec4 &	operator[]( int index ) const;
	CVec4 &		operator[]( int index );
	CMat4			operator*( const float a ) const;
	CVec4			operator*( const CVec4 &vec ) const;
	CVec3			operator*( const CVec3 &vec ) const;
	CMat4			operator*( const CMat4 &a ) const;
	CMat4			operator+( const CMat4 &a ) const;
	CMat4			operator-( const CMat4 &a ) const;
	CMat4 &		operator*=( const float a );
	CMat4 &		operator*=( const CMat4 &a );
	CMat4 &		operator+=( const CMat4 &a );
	CMat4 &		operator-=( const CMat4 &a );

	friend CMat4	operator*( const float a, const CMat4 &mat );
	friend CVec4	operator*( const CVec4 &vec, const CMat4 &mat );
	friend CVec3	operator*( const CVec3 &vec, const CMat4 &mat );
	friend CVec4 &	operator*=( CVec4 &vec, const CMat4 &mat );
	friend CVec3 &	operator*=( CVec3 &vec, const CMat4 &mat );

	bool			compare( const CMat4 &a ) const;						// exact compare, no epsilon
	bool			compare( const CMat4 &a, const float epsilon ) const;	// compare with epsilon
	bool			operator==( const CMat4 &a ) const;					// exact compare, no epsilon
	bool			operator!=( const CMat4 &a ) const;					// exact compare, no epsilon

	void			zero( void );
	void			identity( void );
	bool			isidentity( const float epsilon = MATRIX_EPSILON ) const;
	bool			isSymmetric( const float epsilon = MATRIX_EPSILON ) const;
	bool			isDiagonal( const float epsilon = MATRIX_EPSILON ) const;
	bool			isRotated( void ) const;

	void			projectVector( const CVec4 &src, CVec4 &dst ) const;
	void			unprojectVector( const CVec4 &src, CVec4 &dst ) const;

	float			trace( void ) const;
	float			determinant( void ) const;
	CMat4			transpose( void ) const;	// returns transpose
	CMat4 &		transposeSelf( void );
	CMat4			inverse( void ) const;		// returns the inverse ( m * m.inverse() = identity )
	bool			inverseSelf( void );		// returns false if determinant is zero
	CMat4			inverseFast( void ) const;	// returns the inverse ( m * m.inverse() = identity )
	bool			inverseFastSelf( void );	// returns false if determinant is zero
	CMat4			transposeMultiply( const CMat4 &b ) const;

	int				getDimension( void ) const;

	const float *	toFloatPtr( void ) const;
	float *			toFloatPtr( void );
	const char *	toString( int precision = 2 ) const;

private:
	CVec4			mat[ 4 ];
};

extern CMat4 mat4_zero;
extern CMat4 mat4_identity;
#define mat4_default	mat4_identity

Z_INLINE CMat4::CMat4( void ) {
}

Z_INLINE CMat4::CMat4( const CVec4 &x, const CVec4 &y, const CVec4 &z, const CVec4 &w ) {
	mat[ 0 ] = x;
	mat[ 1 ] = y;
	mat[ 2 ] = z;
	mat[ 3 ] = w;
}

Z_INLINE CMat4::CMat4( const float xx, const float xy, const float xz, const float xw,
							const float yx, const float yy, const float yz, const float yw,
							const float zx, const float zy, const float zz, const float zw,
							const float wx, const float wy, const float wz, const float ww ) {
	mat[0][0] = xx; mat[0][1] = xy; mat[0][2] = xz; mat[0][3] = xw;
	mat[1][0] = yx; mat[1][1] = yy; mat[1][2] = yz; mat[1][3] = yw;
	mat[2][0] = zx; mat[2][1] = zy; mat[2][2] = zz; mat[2][3] = zw;
	mat[3][0] = wx; mat[3][1] = wy; mat[3][2] = wz; mat[3][3] = ww;
}

Z_INLINE CMat4::CMat4( const CMat3 &rotation, const CVec3 &translation ) {
	// NOTE: CMat3 is transposed because it is column-major
	mat[ 0 ][ 0 ] = rotation[0][0];
	mat[ 0 ][ 1 ] = rotation[1][0];
	mat[ 0 ][ 2 ] = rotation[2][0];
	mat[ 0 ][ 3 ] = translation[0];
	mat[ 1 ][ 0 ] = rotation[0][1];
	mat[ 1 ][ 1 ] = rotation[1][1];
	mat[ 1 ][ 2 ] = rotation[2][1];
	mat[ 1 ][ 3 ] = translation[1];
	mat[ 2 ][ 0 ] = rotation[0][2];
	mat[ 2 ][ 1 ] = rotation[1][2];
	mat[ 2 ][ 2 ] = rotation[2][2];
	mat[ 2 ][ 3 ] = translation[2];
	mat[ 3 ][ 0 ] = 0.0f;
	mat[ 3 ][ 1 ] = 0.0f;
	mat[ 3 ][ 2 ] = 0.0f;
	mat[ 3 ][ 3 ] = 1.0f;
}

Z_INLINE CMat4::CMat4( const float src[ 4 ][ 4 ] ) {
	memcpy( mat, src, 4 * 4 * sizeof( float ) );
}

Z_INLINE const CVec4 &CMat4::operator[]( int index ) const {
	//assert( ( index >= 0 ) && ( index < 4 ) );
	return mat[ index ];
}

Z_INLINE CVec4 &CMat4::operator[]( int index ) {
	//assert( ( index >= 0 ) && ( index < 4 ) );
	return mat[ index ];
}

Z_INLINE CMat4 CMat4::operator*( const float a ) const {
	return CMat4(
		mat[0].x * a, mat[0].y * a, mat[0].z * a, mat[0].w * a,
		mat[1].x * a, mat[1].y * a, mat[1].z * a, mat[1].w * a,
		mat[2].x * a, mat[2].y * a, mat[2].z * a, mat[2].w * a,
		mat[3].x * a, mat[3].y * a, mat[3].z * a, mat[3].w * a );
}

Z_INLINE CVec4 CMat4::operator*( const CVec4 &vec ) const {
	return CVec4(
		mat[ 0 ].x * vec.x + mat[ 0 ].y * vec.y + mat[ 0 ].z * vec.z + mat[ 0 ].w * vec.w,
		mat[ 1 ].x * vec.x + mat[ 1 ].y * vec.y + mat[ 1 ].z * vec.z + mat[ 1 ].w * vec.w,
		mat[ 2 ].x * vec.x + mat[ 2 ].y * vec.y + mat[ 2 ].z * vec.z + mat[ 2 ].w * vec.w,
		mat[ 3 ].x * vec.x + mat[ 3 ].y * vec.y + mat[ 3 ].z * vec.z + mat[ 3 ].w * vec.w );
}

Z_INLINE CVec3 CMat4::operator*( const CVec3 &vec ) const {
	float s = mat[ 3 ].x * vec.x + mat[ 3 ].y * vec.y + mat[ 3 ].z * vec.z + mat[ 3 ].w;
	if ( s == 0.0f ) {
		return CVec3( 0.0f, 0.0f, 0.0f );
	}
	if ( s == 1.0f ) {
		return CVec3(
			mat[ 0 ].x * vec.x + mat[ 0 ].y * vec.y + mat[ 0 ].z * vec.z + mat[ 0 ].w,
			mat[ 1 ].x * vec.x + mat[ 1 ].y * vec.y + mat[ 1 ].z * vec.z + mat[ 1 ].w,
			mat[ 2 ].x * vec.x + mat[ 2 ].y * vec.y + mat[ 2 ].z * vec.z + mat[ 2 ].w );
	}
	else {
		float invS = 1.0f / s;
		return CVec3(
			(mat[ 0 ].x * vec.x + mat[ 0 ].y * vec.y + mat[ 0 ].z * vec.z + mat[ 0 ].w) * invS,
			(mat[ 1 ].x * vec.x + mat[ 1 ].y * vec.y + mat[ 1 ].z * vec.z + mat[ 1 ].w) * invS,
			(mat[ 2 ].x * vec.x + mat[ 2 ].y * vec.y + mat[ 2 ].z * vec.z + mat[ 2 ].w) * invS );
	}
}

Z_INLINE CMat4 CMat4::operator*( const CMat4 &a ) const {
	int i, j;
	const float *m1Ptr, *m2Ptr;
	float *dstPtr;
	CMat4 dst;

	m1Ptr = reinterpret_cast<const float *>(this);
	m2Ptr = reinterpret_cast<const float *>(&a);
	dstPtr = reinterpret_cast<float *>(&dst);

	for ( i = 0; i < 4; i++ ) {
		for ( j = 0; j < 4; j++ ) {
			//*dstPtr = m1Ptr[0] * m2Ptr[ 0 * 4 + j ]
			//		+ m1Ptr[1] * m2Ptr[ 1 * 4 + j ]
			//		+ m1Ptr[2] * m2Ptr[ 2 * 4 + j ]
			////		+ m1Ptr[3] * m2Ptr[ 3 * 4 + j ];
			//dstPtr++;
			dst[i][j] = mat[i][0] * a[0][j] +
							mat[i][1] * a[1][j] +
							mat[i][2] * a[2][j] +
							mat[i][3] * a[3][j] ;
		}
		//m1Ptr += 4;
	}

	return dst;
}

Z_INLINE CMat4 CMat4::operator+( const CMat4 &a ) const {
	return CMat4( 
		mat[0].x + a[0].x, mat[0].y + a[0].y, mat[0].z + a[0].z, mat[0].w + a[0].w,
		mat[1].x + a[1].x, mat[1].y + a[1].y, mat[1].z + a[1].z, mat[1].w + a[1].w,
		mat[2].x + a[2].x, mat[2].y + a[2].y, mat[2].z + a[2].z, mat[2].w + a[2].w,
		mat[3].x + a[3].x, mat[3].y + a[3].y, mat[3].z + a[3].z, mat[3].w + a[3].w );
}
    
Z_INLINE CMat4 CMat4::operator-( const CMat4 &a ) const {
	return CMat4( 
		mat[0].x - a[0].x, mat[0].y - a[0].y, mat[0].z - a[0].z, mat[0].w - a[0].w,
		mat[1].x - a[1].x, mat[1].y - a[1].y, mat[1].z - a[1].z, mat[1].w - a[1].w,
		mat[2].x - a[2].x, mat[2].y - a[2].y, mat[2].z - a[2].z, mat[2].w - a[2].w,
		mat[3].x - a[3].x, mat[3].y - a[3].y, mat[3].z - a[3].z, mat[3].w - a[3].w );
}

Z_INLINE CMat4 &CMat4::operator*=( const float a ) {
	mat[0].x *= a; mat[0].y *= a; mat[0].z *= a; mat[0].w *= a;
	mat[1].x *= a; mat[1].y *= a; mat[1].z *= a; mat[1].w *= a;
	mat[2].x *= a; mat[2].y *= a; mat[2].z *= a; mat[2].w *= a;
	mat[3].x *= a; mat[3].y *= a; mat[3].z *= a; mat[3].w *= a;
    return *this;
}

Z_INLINE CMat4 &CMat4::operator*=( const CMat4 &a ) {
	*this = (*this) * a;
	return *this;
}

Z_INLINE CMat4 &CMat4::operator+=( const CMat4 &a ) {
	mat[0].x += a[0].x; mat[0].y += a[0].y; mat[0].z += a[0].z; mat[0].w += a[0].w;
	mat[1].x += a[1].x; mat[1].y += a[1].y; mat[1].z += a[1].z; mat[1].w += a[1].w;
	mat[2].x += a[2].x; mat[2].y += a[2].y; mat[2].z += a[2].z; mat[2].w += a[2].w;
	mat[3].x += a[3].x; mat[3].y += a[3].y; mat[3].z += a[3].z; mat[3].w += a[3].w;
    return *this;
}

Z_INLINE CMat4 &CMat4::operator-=( const CMat4 &a ) {
	mat[0].x -= a[0].x; mat[0].y -= a[0].y; mat[0].z -= a[0].z; mat[0].w -= a[0].w;
	mat[1].x -= a[1].x; mat[1].y -= a[1].y; mat[1].z -= a[1].z; mat[1].w -= a[1].w;
	mat[2].x -= a[2].x; mat[2].y -= a[2].y; mat[2].z -= a[2].z; mat[2].w -= a[2].w;
	mat[3].x -= a[3].x; mat[3].y -= a[3].y; mat[3].z -= a[3].z; mat[3].w -= a[3].w;
    return *this;
}

Z_INLINE CMat4 operator*( const float a, const CMat4 &mat ) {
	return mat * a;
}

Z_INLINE CVec4 operator*( const CVec4 &vec, const CMat4 &mat ) {
	return CVec4(vec[0]*mat[0][0] + vec[1]*mat[1][0] + vec[2]*mat[2][0] + vec[3]*mat[3][0],
				 vec[0]*mat[0][1] + vec[1]*mat[1][1] + vec[2]*mat[2][1] + vec[3]*mat[3][1],
				 vec[0]*mat[0][2] + vec[1]*mat[1][2] + vec[2]*mat[2][2] + vec[3]*mat[3][2],
				 vec[0]*mat[0][3] + vec[1]*mat[1][3] + vec[2]*mat[2][3] + vec[3]*mat[3][3]);
}

Z_INLINE CVec3 operator*( const CVec3 &vec, const CMat4 &mat ) {
	return CVec3(vec[0]*mat[0][0] + vec[1]*mat[1][0] + vec[2]*mat[2][0] + mat[3][0],
				 vec[0]*mat[0][1] + vec[1]*mat[1][1] + vec[2]*mat[2][1] + mat[3][1],
				 vec[0]*mat[0][2] + vec[1]*mat[1][2] + vec[2]*mat[2][2] + mat[3][2]);
}

Z_INLINE CVec4 &operator*=( CVec4 &vec, const CMat4 &mat ) {
	vec = vec * mat;
	return vec;
}

Z_INLINE CVec3 &operator*=( CVec3 &vec, const CMat4 &mat ) {
	vec = vec * mat;
	return vec;
}

Z_INLINE bool CMat4::compare( const CMat4 &a ) const {
	DWORD i;
	const float *ptr1, *ptr2;

	ptr1 = reinterpret_cast<const float *>(mat);
	ptr2 = reinterpret_cast<const float *>(a.mat);
	for ( i = 0; i < 4*4; i++ ) {
		if ( ptr1[i] != ptr2[i] ) {
			return false;
		}
	}
	return true;
}

Z_INLINE bool CMat4::compare( const CMat4 &a, const float epsilon ) const {
	DWORD i;
	const float *ptr1, *ptr2;

	ptr1 = reinterpret_cast<const float *>(mat);
	ptr2 = reinterpret_cast<const float *>(a.mat);
	for ( i = 0; i < 4*4; i++ ) {
		if ( CZMath::Fabs( ptr1[i] - ptr2[i] ) > epsilon ) {
			return false;
		}
	}
	return true;
}

Z_INLINE bool CMat4::operator==( const CMat4 &a ) const {
	return compare( a );
}

Z_INLINE bool CMat4::operator!=( const CMat4 &a ) const {
	return !compare( a );
}

Z_INLINE void CMat4::zero( void ) {
	memset( mat, 0, sizeof( CMat4 ) );
}

Z_INLINE void CMat4::identity( void ) {
	*this = mat4_identity;
}

Z_INLINE bool CMat4::isidentity( const float epsilon ) const {
	return compare( mat4_identity, epsilon );
}

Z_INLINE bool CMat4::isSymmetric( const float epsilon ) const {
	for ( int i = 1; i < 4; i++ ) {
		for ( int j = 0; j < i; j++ ) {
			if ( CZMath::Fabs( mat[i][j] - mat[j][i] ) > epsilon ) {
				return false;
			}
		}
	}
	return true;
}

Z_INLINE bool CMat4::isDiagonal( const float epsilon ) const {
	for ( int i = 0; i < 4; i++ ) {
		for ( int j = 0; j < 4; j++ ) {
			if ( i != j && CZMath::Fabs( mat[i][j] ) > epsilon ) {
				return false;
			}
		}
	}
	return true;
}

Z_INLINE bool CMat4::isRotated( void ) const {
	if ( !mat[ 0 ][ 1 ] && !mat[ 0 ][ 2 ] &&
		!mat[ 1 ][ 0 ] && !mat[ 1 ][ 2 ] &&
		!mat[ 2 ][ 0 ] && !mat[ 2 ][ 1 ] ) {
		return false;
	}
	return true;
}

Z_INLINE void CMat4::projectVector( const CVec4 &src, CVec4 &dst ) const {
	dst.x = src * mat[ 0 ];
	dst.y = src * mat[ 1 ];
	dst.z = src * mat[ 2 ];
	dst.w = src * mat[ 3 ];
}

Z_INLINE void CMat4::unprojectVector( const CVec4 &src, CVec4 &dst ) const {
	dst = mat[ 0 ] * src.x + mat[ 1 ] * src.y + mat[ 2 ] * src.z + mat[ 3 ] * src.w;
}

Z_INLINE float CMat4::trace( void ) const {
	return ( mat[0][0] + mat[1][1] + mat[2][2] + mat[3][3] );
}

Z_INLINE CMat4 CMat4::inverse( void ) const {
	CMat4 invMat;

	invMat = *this;
	int r = invMat.inverseSelf();
	assert( r );
	return invMat;
}

Z_INLINE CMat4 CMat4::inverseFast( void ) const {
	CMat4 invMat;

	invMat = *this;
	int r = invMat.inverseFastSelf();
	assert( r );
	return invMat;
}

Z_INLINE CMat4 CMat3::toMat4( void ) const {
	// NOTE: CMat3 is transposed because it is column-major
	return CMat4(	mat[0][0],	mat[1][0],	mat[2][0],	0.0f,
					mat[0][1],	mat[1][1],	mat[2][1],	0.0f,
					mat[0][2],	mat[1][2],	mat[2][2],	0.0f,
					0.0f,		0.0f,		0.0f,		1.0f );
}

Z_INLINE int CMat4::getDimension( void ) const {
	return 16;
}

Z_INLINE const float *CMat4::toFloatPtr( void ) const {
	return mat[0].toFloatPtr();
}

Z_INLINE float *CMat4::toFloatPtr( void ) {
	return mat[0].toFloatPtr();
}


//===============================================================
//
//	CMat5 - 5x5 matrix
//
//===============================================================

class CMat5 {
public:
					CMat5( void );
					explicit CMat5( const CVec5 &v0, const CVec5 &v1, const CVec5 &v2, const CVec5 &v3, const CVec5 &v4 );
					explicit CMat5( const float src[ 5 ][ 5 ] );

	const CVec5 &	operator[]( int index ) const;
	CVec5 &		operator[]( int index );
	CMat5			operator*( const float a ) const;
	CVec5			operator*( const CVec5 &vec ) const;
	CMat5			operator*( const CMat5 &a ) const;
	CMat5			operator+( const CMat5 &a ) const;
	CMat5			operator-( const CMat5 &a ) const;
	CMat5 &		operator*=( const float a );
	CMat5 &		operator*=( const CMat5 &a );
	CMat5 &		operator+=( const CMat5 &a );
	CMat5 &		operator-=( const CMat5 &a );

	friend CMat5	operator*( const float a, const CMat5 &mat );
	friend CVec5	operator*( const CVec5 &vec, const CMat5 &mat );
	friend CVec5 &	operator*=( CVec5 &vec, const CMat5 &mat );

	bool			compare( const CMat5 &a ) const;						// exact compare, no epsilon
	bool			compare( const CMat5 &a, const float epsilon ) const;	// compare with epsilon
	bool			operator==( const CMat5 &a ) const;					// exact compare, no epsilon
	bool			operator!=( const CMat5 &a ) const;					// exact compare, no epsilon

	void			zero( void );
	void			identity( void );
	bool			isidentity( const float epsilon = MATRIX_EPSILON ) const;
	bool			isSymmetric( const float epsilon = MATRIX_EPSILON ) const;
	bool			isDiagonal( const float epsilon = MATRIX_EPSILON ) const;

	float			trace( void ) const;
	float			determinant( void ) const;
	CMat5			transpose( void ) const;	// returns transpose
	CMat5 &		transposeSelf( void );
	CMat5			inverse( void ) const;		// returns the inverse ( m * m.inverse() = identity )
	bool			inverseSelf( void );		// returns false if determinant is zero
	CMat5			inverseFast( void ) const;	// returns the inverse ( m * m.inverse() = identity )
	bool			inverseFastSelf( void );	// returns false if determinant is zero

	int				getDimension( void ) const;

	const float *	toFloatPtr( void ) const;
	float *			toFloatPtr( void );
	//const char *	toString( int precision = 2 ) const;

private:
	CVec5			mat[ 5 ];
};

extern CMat5 mat5_zero;
extern CMat5 mat5_identity;
#define mat5_default	mat5_identity

Z_INLINE CMat5::CMat5( void ) {
}

Z_INLINE CMat5::CMat5( const float src[ 5 ][ 5 ] ) {
	memcpy( mat, src, 5 * 5 * sizeof( float ) );
}

Z_INLINE CMat5::CMat5( const CVec5 &v0, const CVec5 &v1, const CVec5 &v2, const CVec5 &v3, const CVec5 &v4 ) {
	mat[0] = v0;
	mat[1] = v1;
	mat[2] = v2;
	mat[3] = v3;
	mat[4] = v4;
}

Z_INLINE const CVec5 &CMat5::operator[]( int index ) const {
	//assert( ( index >= 0 ) && ( index < 5 ) );
	return mat[ index ];
}

Z_INLINE CVec5 &CMat5::operator[]( int index ) {
	//assert( ( index >= 0 ) && ( index < 5 ) );
	return mat[ index ];
}

Z_INLINE CMat5 CMat5::operator*( const CMat5 &a ) const {
	int i, j;
	const float *m1Ptr, *m2Ptr;
	float *dstPtr;
	CMat5 dst;

	m1Ptr = reinterpret_cast<const float *>(this);
	m2Ptr = reinterpret_cast<const float *>(&a);
	dstPtr = reinterpret_cast<float *>(&dst);

	for ( i = 0; i < 5; i++ ) {
		for ( j = 0; j < 5; j++ ) {
			*dstPtr = m1Ptr[0] * m2Ptr[ 0 * 5 + j ]
					+ m1Ptr[1] * m2Ptr[ 1 * 5 + j ]
					+ m1Ptr[2] * m2Ptr[ 2 * 5 + j ]
					+ m1Ptr[3] * m2Ptr[ 3 * 5 + j ]
					+ m1Ptr[4] * m2Ptr[ 4 * 5 + j ];
			dstPtr++;
		}
		m1Ptr += 5;
	}
	return dst;
}

Z_INLINE CMat5 CMat5::operator*( const float a ) const {
	return CMat5(
		CVec5( mat[0][0] * a, mat[0][1] * a, mat[0][2] * a, mat[0][3] * a, mat[0][4] * a ),
		CVec5( mat[1][0] * a, mat[1][1] * a, mat[1][2] * a, mat[1][3] * a, mat[1][4] * a ),
		CVec5( mat[2][0] * a, mat[2][1] * a, mat[2][2] * a, mat[2][3] * a, mat[2][4] * a ),
		CVec5( mat[3][0] * a, mat[3][1] * a, mat[3][2] * a, mat[3][3] * a, mat[3][4] * a ),
		CVec5( mat[4][0] * a, mat[4][1] * a, mat[4][2] * a, mat[4][3] * a, mat[4][4] * a ) );
}

Z_INLINE CVec5 CMat5::operator*( const CVec5 &vec ) const {
	return CVec5(
		mat[0][0] * vec[0] + mat[0][1] * vec[1] + mat[0][2] * vec[2] + mat[0][3] * vec[3] + mat[0][4] * vec[4],
		mat[1][0] * vec[0] + mat[1][1] * vec[1] + mat[1][2] * vec[2] + mat[1][3] * vec[3] + mat[1][4] * vec[4],
		mat[2][0] * vec[0] + mat[2][1] * vec[1] + mat[2][2] * vec[2] + mat[2][3] * vec[3] + mat[2][4] * vec[4],
		mat[3][0] * vec[0] + mat[3][1] * vec[1] + mat[3][2] * vec[2] + mat[3][3] * vec[3] + mat[3][4] * vec[4],
		mat[4][0] * vec[0] + mat[4][1] * vec[1] + mat[4][2] * vec[2] + mat[4][3] * vec[3] + mat[4][4] * vec[4] );
}

Z_INLINE CMat5 CMat5::operator+( const CMat5 &a ) const {
	return CMat5(
		CVec5( mat[0][0] + a[0][0], mat[0][1] + a[0][1], mat[0][2] + a[0][2], mat[0][3] + a[0][3], mat[0][4] + a[0][4] ),
		CVec5( mat[1][0] + a[1][0], mat[1][1] + a[1][1], mat[1][2] + a[1][2], mat[1][3] + a[1][3], mat[1][4] + a[1][4] ),
		CVec5( mat[2][0] + a[2][0], mat[2][1] + a[2][1], mat[2][2] + a[2][2], mat[2][3] + a[2][3], mat[2][4] + a[2][4] ),
		CVec5( mat[3][0] + a[3][0], mat[3][1] + a[3][1], mat[3][2] + a[3][2], mat[3][3] + a[3][3], mat[3][4] + a[3][4] ),
		CVec5( mat[4][0] + a[4][0], mat[4][1] + a[4][1], mat[4][2] + a[4][2], mat[4][3] + a[4][3], mat[4][4] + a[4][4] ) );
}

Z_INLINE CMat5 CMat5::operator-( const CMat5 &a ) const {
	return CMat5(
		CVec5( mat[0][0] - a[0][0], mat[0][1] - a[0][1], mat[0][2] - a[0][2], mat[0][3] - a[0][3], mat[0][4] - a[0][4] ),
		CVec5( mat[1][0] - a[1][0], mat[1][1] - a[1][1], mat[1][2] - a[1][2], mat[1][3] - a[1][3], mat[1][4] - a[1][4] ),
		CVec5( mat[2][0] - a[2][0], mat[2][1] - a[2][1], mat[2][2] - a[2][2], mat[2][3] - a[2][3], mat[2][4] - a[2][4] ),
		CVec5( mat[3][0] - a[3][0], mat[3][1] - a[3][1], mat[3][2] - a[3][2], mat[3][3] - a[3][3], mat[3][4] - a[3][4] ),
		CVec5( mat[4][0] - a[4][0], mat[4][1] - a[4][1], mat[4][2] - a[4][2], mat[4][3] - a[4][3], mat[4][4] - a[4][4] ) );
}

Z_INLINE CMat5 &CMat5::operator*=( const float a ) {
	mat[0][0] *= a; mat[0][1] *= a; mat[0][2] *= a; mat[0][3] *= a; mat[0][4] *= a;
	mat[1][0] *= a; mat[1][1] *= a; mat[1][2] *= a; mat[1][3] *= a; mat[1][4] *= a;
	mat[2][0] *= a; mat[2][1] *= a; mat[2][2] *= a; mat[2][3] *= a; mat[2][4] *= a;
	mat[3][0] *= a; mat[3][1] *= a; mat[3][2] *= a; mat[3][3] *= a; mat[3][4] *= a;
	mat[4][0] *= a; mat[4][1] *= a; mat[4][2] *= a; mat[4][3] *= a; mat[4][4] *= a;
	return *this;
}

Z_INLINE CMat5 &CMat5::operator*=( const CMat5 &a ) {
	*this = *this * a;
	return *this;
}

Z_INLINE CMat5 &CMat5::operator+=( const CMat5 &a ) {
	mat[0][0] += a[0][0]; mat[0][1] += a[0][1]; mat[0][2] += a[0][2]; mat[0][3] += a[0][3]; mat[0][4] += a[0][4];
	mat[1][0] += a[1][0]; mat[1][1] += a[1][1]; mat[1][2] += a[1][2]; mat[1][3] += a[1][3]; mat[1][4] += a[1][4];
	mat[2][0] += a[2][0]; mat[2][1] += a[2][1]; mat[2][2] += a[2][2]; mat[2][3] += a[2][3]; mat[2][4] += a[2][4];
	mat[3][0] += a[3][0]; mat[3][1] += a[3][1]; mat[3][2] += a[3][2]; mat[3][3] += a[3][3]; mat[3][4] += a[3][4];
	mat[4][0] += a[4][0]; mat[4][1] += a[4][1]; mat[4][2] += a[4][2]; mat[4][3] += a[4][3]; mat[4][4] += a[4][4];
	return *this;
}

Z_INLINE CMat5 &CMat5::operator-=( const CMat5 &a ) {
	mat[0][0] -= a[0][0]; mat[0][1] -= a[0][1]; mat[0][2] -= a[0][2]; mat[0][3] -= a[0][3]; mat[0][4] -= a[0][4];
	mat[1][0] -= a[1][0]; mat[1][1] -= a[1][1]; mat[1][2] -= a[1][2]; mat[1][3] -= a[1][3]; mat[1][4] -= a[1][4];
	mat[2][0] -= a[2][0]; mat[2][1] -= a[2][1]; mat[2][2] -= a[2][2]; mat[2][3] -= a[2][3]; mat[2][4] -= a[2][4];
	mat[3][0] -= a[3][0]; mat[3][1] -= a[3][1]; mat[3][2] -= a[3][2]; mat[3][3] -= a[3][3]; mat[3][4] -= a[3][4];
	mat[4][0] -= a[4][0]; mat[4][1] -= a[4][1]; mat[4][2] -= a[4][2]; mat[4][3] -= a[4][3]; mat[4][4] -= a[4][4];
	return *this;
}

Z_INLINE CVec5 operator*( const CVec5 &vec, const CMat5 &mat ) {
	return mat * vec;
}

Z_INLINE CMat5 operator*( const float a, CMat5 const &mat ) {
	return mat * a;
}

Z_INLINE CVec5 &operator*=( CVec5 &vec, const CMat5 &mat ) {
	vec = mat * vec;
	return vec;
}

Z_INLINE bool CMat5::compare( const CMat5 &a ) const {
	DWORD i;
	const float *ptr1, *ptr2;

	ptr1 = reinterpret_cast<const float *>(mat);
	ptr2 = reinterpret_cast<const float *>(a.mat);
	for ( i = 0; i < 5*5; i++ ) {
		if ( ptr1[i] != ptr2[i] ) {
			return false;
		}
	}
	return true;
}

Z_INLINE bool CMat5::compare( const CMat5 &a, const float epsilon ) const {
	DWORD i;
	const float *ptr1, *ptr2;

	ptr1 = reinterpret_cast<const float *>(mat);
	ptr2 = reinterpret_cast<const float *>(a.mat);
	for ( i = 0; i < 5*5; i++ ) {
		if ( CZMath::Fabs( ptr1[i] - ptr2[i] ) > epsilon ) {
			return false;
		}
	}
	return true;
}

Z_INLINE bool CMat5::operator==( const CMat5 &a ) const {
	return compare( a );
}

Z_INLINE bool CMat5::operator!=( const CMat5 &a ) const {
	return !compare( a );
}

Z_INLINE void CMat5::zero( void ) {
	memset( mat, 0, sizeof( CMat5 ) );
}

Z_INLINE void CMat5::identity( void ) {
	*this = mat5_identity;
}

Z_INLINE bool CMat5::isidentity( const float epsilon ) const {
	return compare( mat5_identity, epsilon );
}

Z_INLINE bool CMat5::isSymmetric( const float epsilon ) const {
	for ( int i = 1; i < 5; i++ ) {
		for ( int j = 0; j < i; j++ ) {
			if ( CZMath::Fabs( mat[i][j] - mat[j][i] ) > epsilon ) {
				return false;
			}
		}
	}
	return true;
}

Z_INLINE bool CMat5::isDiagonal( const float epsilon ) const {
	for ( int i = 0; i < 5; i++ ) {
		for ( int j = 0; j < 5; j++ ) {
			if ( i != j && CZMath::Fabs( mat[i][j] ) > epsilon ) {
				return false;
			}
		}
	}
	return true;
}

Z_INLINE float CMat5::trace( void ) const {
	return ( mat[0][0] + mat[1][1] + mat[2][2] + mat[3][3] + mat[4][4] );
}

Z_INLINE CMat5 CMat5::inverse( void ) const {
	CMat5 invMat;

	invMat = *this;
	int r = invMat.inverseSelf();
	assert( r );
	return invMat;
}

Z_INLINE CMat5 CMat5::inverseFast( void ) const {
	CMat5 invMat;

	invMat = *this;
	int r = invMat.inverseFastSelf();
	assert( r );
	return invMat;
}

Z_INLINE int CMat5::getDimension( void ) const {
	return 25;
}

Z_INLINE const float *CMat5::toFloatPtr( void ) const {
	return mat[0].toFloatPtr();
}

Z_INLINE float *CMat5::toFloatPtr( void ) {
	return mat[0].toFloatPtr();
}


//===============================================================
//
//	CMat6 - 6x6 matrix
//
//===============================================================

class CMat6 {
public:
					CMat6( void );
					explicit CMat6( const CVec6 &v0, const CVec6 &v1, const CVec6 &v2, const CVec6 &v3, const CVec6 &v4, const CVec6 &v5 );
					explicit CMat6( const CMat3 &m0, const CMat3 &m1, const CMat3 &m2, const CMat3 &m3 );
					explicit CMat6( const float src[ 6 ][ 6 ] );

	const CVec6 &	operator[]( int index ) const;
	CVec6 &		operator[]( int index );
	CMat6			operator*( const float a ) const;
	CVec6			operator*( const CVec6 &vec ) const;
	CMat6			operator*( const CMat6 &a ) const;
	CMat6			operator+( const CMat6 &a ) const;
	CMat6			operator-( const CMat6 &a ) const;
	CMat6 &		operator*=( const float a );
	CMat6 &		operator*=( const CMat6 &a );
	CMat6 &		operator+=( const CMat6 &a );
	CMat6 &		operator-=( const CMat6 &a );

	friend CMat6	operator*( const float a, const CMat6 &mat );
	friend CVec6	operator*( const CVec6 &vec, const CMat6 &mat );
	friend CVec6 &	operator*=( CVec6 &vec, const CMat6 &mat );

	bool			compare( const CMat6 &a ) const;						// exact compare, no epsilon
	bool			compare( const CMat6 &a, const float epsilon ) const;	// compare with epsilon
	bool			operator==( const CMat6 &a ) const;					// exact compare, no epsilon
	bool			operator!=( const CMat6 &a ) const;					// exact compare, no epsilon

	void			zero( void );
	void			identity( void );
	bool			isidentity( const float epsilon = MATRIX_EPSILON ) const;
	bool			isSymmetric( const float epsilon = MATRIX_EPSILON ) const;
	bool			isDiagonal( const float epsilon = MATRIX_EPSILON ) const;

	CMat3			subMat3( int n ) const;
	float			trace( void ) const;
	float			determinant( void ) const;
	CMat6			transpose( void ) const;	// returns transpose
	CMat6 &		transposeSelf( void );
	CMat6			inverse( void ) const;		// returns the inverse ( m * m.inverse() = identity )
	bool			inverseSelf( void );		// returns false if determinant is zero
	CMat6			inverseFast( void ) const;	// returns the inverse ( m * m.inverse() = identity )
	bool			inverseFastSelf( void );	// returns false if determinant is zero

	int				getDimension( void ) const;

	const float *	toFloatPtr( void ) const;
	float *			toFloatPtr( void );
	//const char *	toString( int precision = 2 ) const;

private:
	CVec6			mat[ 6 ];
};

extern CMat6 mat6_zero;
extern CMat6 mat6_identity;
#define mat6_default	mat6_identity

Z_INLINE CMat6::CMat6( void ) {
}

Z_INLINE CMat6::CMat6( const CMat3 &m0, const CMat3 &m1, const CMat3 &m2, const CMat3 &m3 ) {
	mat[0] = CVec6( m0[0][0], m0[0][1], m0[0][2], m1[0][0], m1[0][1], m1[0][2] );
	mat[1] = CVec6( m0[1][0], m0[1][1], m0[1][2], m1[1][0], m1[1][1], m1[1][2] );
	mat[2] = CVec6( m0[2][0], m0[2][1], m0[2][2], m1[2][0], m1[2][1], m1[2][2] );
	mat[3] = CVec6( m2[0][0], m2[0][1], m2[0][2], m3[0][0], m3[0][1], m3[0][2] );
	mat[4] = CVec6( m2[1][0], m2[1][1], m2[1][2], m3[1][0], m3[1][1], m3[1][2] );
	mat[5] = CVec6( m2[2][0], m2[2][1], m2[2][2], m3[2][0], m3[2][1], m3[2][2] );
}

Z_INLINE CMat6::CMat6( const CVec6 &v0, const CVec6 &v1, const CVec6 &v2, const CVec6 &v3, const CVec6 &v4, const CVec6 &v5 ) {
	mat[0] = v0;
	mat[1] = v1;
	mat[2] = v2;
	mat[3] = v3;
	mat[4] = v4;
	mat[5] = v5;
}

Z_INLINE CMat6::CMat6( const float src[ 6 ][ 6 ] ) {
	memcpy( mat, src, 6 * 6 * sizeof( float ) );
}

Z_INLINE const CVec6 &CMat6::operator[]( int index ) const {
	//assert( ( index >= 0 ) && ( index < 6 ) );
	return mat[ index ];
}

Z_INLINE CVec6 &CMat6::operator[]( int index ) {
	//assert( ( index >= 0 ) && ( index < 6 ) );
	return mat[ index ];
}

Z_INLINE CMat6 CMat6::operator*( const CMat6 &a ) const {
	int i, j;
	const float *m1Ptr, *m2Ptr;
	float *dstPtr;
	CMat6 dst;

	m1Ptr = reinterpret_cast<const float *>(this);
	m2Ptr = reinterpret_cast<const float *>(&a);
	dstPtr = reinterpret_cast<float *>(&dst);

	for ( i = 0; i < 6; i++ ) {
		for ( j = 0; j < 6; j++ ) {
			*dstPtr = m1Ptr[0] * m2Ptr[ 0 * 6 + j ]
					+ m1Ptr[1] * m2Ptr[ 1 * 6 + j ]
					+ m1Ptr[2] * m2Ptr[ 2 * 6 + j ]
					+ m1Ptr[3] * m2Ptr[ 3 * 6 + j ]
					+ m1Ptr[4] * m2Ptr[ 4 * 6 + j ]
					+ m1Ptr[5] * m2Ptr[ 5 * 6 + j ];
			dstPtr++;
		}
		m1Ptr += 6;
	}
	return dst;
}

Z_INLINE CMat6 CMat6::operator*( const float a ) const {
	return CMat6(
		CVec6( mat[0][0] * a, mat[0][1] * a, mat[0][2] * a, mat[0][3] * a, mat[0][4] * a, mat[0][5] * a ),
		CVec6( mat[1][0] * a, mat[1][1] * a, mat[1][2] * a, mat[1][3] * a, mat[1][4] * a, mat[1][5] * a ),
		CVec6( mat[2][0] * a, mat[2][1] * a, mat[2][2] * a, mat[2][3] * a, mat[2][4] * a, mat[2][5] * a ),
		CVec6( mat[3][0] * a, mat[3][1] * a, mat[3][2] * a, mat[3][3] * a, mat[3][4] * a, mat[3][5] * a ),
		CVec6( mat[4][0] * a, mat[4][1] * a, mat[4][2] * a, mat[4][3] * a, mat[4][4] * a, mat[4][5] * a ),
		CVec6( mat[5][0] * a, mat[5][1] * a, mat[5][2] * a, mat[5][3] * a, mat[5][4] * a, mat[5][5] * a ) );
}

Z_INLINE CVec6 CMat6::operator*( const CVec6 &vec ) const {
	return CVec6(
		mat[0][0] * vec[0] + mat[0][1] * vec[1] + mat[0][2] * vec[2] + mat[0][3] * vec[3] + mat[0][4] * vec[4] + mat[0][5] * vec[5],
		mat[1][0] * vec[0] + mat[1][1] * vec[1] + mat[1][2] * vec[2] + mat[1][3] * vec[3] + mat[1][4] * vec[4] + mat[1][5] * vec[5],
		mat[2][0] * vec[0] + mat[2][1] * vec[1] + mat[2][2] * vec[2] + mat[2][3] * vec[3] + mat[2][4] * vec[4] + mat[2][5] * vec[5],
		mat[3][0] * vec[0] + mat[3][1] * vec[1] + mat[3][2] * vec[2] + mat[3][3] * vec[3] + mat[3][4] * vec[4] + mat[3][5] * vec[5],
		mat[4][0] * vec[0] + mat[4][1] * vec[1] + mat[4][2] * vec[2] + mat[4][3] * vec[3] + mat[4][4] * vec[4] + mat[4][5] * vec[5],
		mat[5][0] * vec[0] + mat[5][1] * vec[1] + mat[5][2] * vec[2] + mat[5][3] * vec[3] + mat[5][4] * vec[4] + mat[5][5] * vec[5] );
}

Z_INLINE CMat6 CMat6::operator+( const CMat6 &a ) const {
	return CMat6(
		CVec6( mat[0][0] + a[0][0], mat[0][1] + a[0][1], mat[0][2] + a[0][2], mat[0][3] + a[0][3], mat[0][4] + a[0][4], mat[0][5] + a[0][5] ),
		CVec6( mat[1][0] + a[1][0], mat[1][1] + a[1][1], mat[1][2] + a[1][2], mat[1][3] + a[1][3], mat[1][4] + a[1][4], mat[1][5] + a[1][5] ),
		CVec6( mat[2][0] + a[2][0], mat[2][1] + a[2][1], mat[2][2] + a[2][2], mat[2][3] + a[2][3], mat[2][4] + a[2][4], mat[2][5] + a[2][5] ),
		CVec6( mat[3][0] + a[3][0], mat[3][1] + a[3][1], mat[3][2] + a[3][2], mat[3][3] + a[3][3], mat[3][4] + a[3][4], mat[3][5] + a[3][5] ),
		CVec6( mat[4][0] + a[4][0], mat[4][1] + a[4][1], mat[4][2] + a[4][2], mat[4][3] + a[4][3], mat[4][4] + a[4][4], mat[4][5] + a[4][5] ),
		CVec6( mat[5][0] + a[5][0], mat[5][1] + a[5][1], mat[5][2] + a[5][2], mat[5][3] + a[5][3], mat[5][4] + a[5][4], mat[5][5] + a[5][5] ) );
}

Z_INLINE CMat6 CMat6::operator-( const CMat6 &a ) const {
	return CMat6(
		CVec6( mat[0][0] - a[0][0], mat[0][1] - a[0][1], mat[0][2] - a[0][2], mat[0][3] - a[0][3], mat[0][4] - a[0][4], mat[0][5] - a[0][5] ),
		CVec6( mat[1][0] - a[1][0], mat[1][1] - a[1][1], mat[1][2] - a[1][2], mat[1][3] - a[1][3], mat[1][4] - a[1][4], mat[1][5] - a[1][5] ),
		CVec6( mat[2][0] - a[2][0], mat[2][1] - a[2][1], mat[2][2] - a[2][2], mat[2][3] - a[2][3], mat[2][4] - a[2][4], mat[2][5] - a[2][5] ),
		CVec6( mat[3][0] - a[3][0], mat[3][1] - a[3][1], mat[3][2] - a[3][2], mat[3][3] - a[3][3], mat[3][4] - a[3][4], mat[3][5] - a[3][5] ),
		CVec6( mat[4][0] - a[4][0], mat[4][1] - a[4][1], mat[4][2] - a[4][2], mat[4][3] - a[4][3], mat[4][4] - a[4][4], mat[4][5] - a[4][5] ),
		CVec6( mat[5][0] - a[5][0], mat[5][1] - a[5][1], mat[5][2] - a[5][2], mat[5][3] - a[5][3], mat[5][4] - a[5][4], mat[5][5] - a[5][5] ) );
}

Z_INLINE CMat6 &CMat6::operator*=( const float a ) {
	mat[0][0] *= a; mat[0][1] *= a; mat[0][2] *= a; mat[0][3] *= a; mat[0][4] *= a; mat[0][5] *= a;
	mat[1][0] *= a; mat[1][1] *= a; mat[1][2] *= a; mat[1][3] *= a; mat[1][4] *= a; mat[1][5] *= a;
	mat[2][0] *= a; mat[2][1] *= a; mat[2][2] *= a; mat[2][3] *= a; mat[2][4] *= a; mat[2][5] *= a;
	mat[3][0] *= a; mat[3][1] *= a; mat[3][2] *= a; mat[3][3] *= a; mat[3][4] *= a; mat[3][5] *= a;
	mat[4][0] *= a; mat[4][1] *= a; mat[4][2] *= a; mat[4][3] *= a; mat[4][4] *= a; mat[4][5] *= a;
	mat[5][0] *= a; mat[5][1] *= a; mat[5][2] *= a; mat[5][3] *= a; mat[5][4] *= a; mat[5][5] *= a;
	return *this;
}

Z_INLINE CMat6 &CMat6::operator*=( const CMat6 &a ) {
	*this = *this * a;
	return *this;
}

Z_INLINE CMat6 &CMat6::operator+=( const CMat6 &a ) {
	mat[0][0] += a[0][0]; mat[0][1] += a[0][1]; mat[0][2] += a[0][2]; mat[0][3] += a[0][3]; mat[0][4] += a[0][4]; mat[0][5] += a[0][5];
	mat[1][0] += a[1][0]; mat[1][1] += a[1][1]; mat[1][2] += a[1][2]; mat[1][3] += a[1][3]; mat[1][4] += a[1][4]; mat[1][5] += a[1][5];
	mat[2][0] += a[2][0]; mat[2][1] += a[2][1]; mat[2][2] += a[2][2]; mat[2][3] += a[2][3]; mat[2][4] += a[2][4]; mat[2][5] += a[2][5];
	mat[3][0] += a[3][0]; mat[3][1] += a[3][1]; mat[3][2] += a[3][2]; mat[3][3] += a[3][3]; mat[3][4] += a[3][4]; mat[3][5] += a[3][5];
	mat[4][0] += a[4][0]; mat[4][1] += a[4][1]; mat[4][2] += a[4][2]; mat[4][3] += a[4][3]; mat[4][4] += a[4][4]; mat[4][5] += a[4][5];
	mat[5][0] += a[5][0]; mat[5][1] += a[5][1]; mat[5][2] += a[5][2]; mat[5][3] += a[5][3]; mat[5][4] += a[5][4]; mat[5][5] += a[5][5];
	return *this;
}

Z_INLINE CMat6 &CMat6::operator-=( const CMat6 &a ) {
	mat[0][0] -= a[0][0]; mat[0][1] -= a[0][1]; mat[0][2] -= a[0][2]; mat[0][3] -= a[0][3]; mat[0][4] -= a[0][4]; mat[0][5] -= a[0][5];
	mat[1][0] -= a[1][0]; mat[1][1] -= a[1][1]; mat[1][2] -= a[1][2]; mat[1][3] -= a[1][3]; mat[1][4] -= a[1][4]; mat[1][5] -= a[1][5];
	mat[2][0] -= a[2][0]; mat[2][1] -= a[2][1]; mat[2][2] -= a[2][2]; mat[2][3] -= a[2][3]; mat[2][4] -= a[2][4]; mat[2][5] -= a[2][5];
	mat[3][0] -= a[3][0]; mat[3][1] -= a[3][1]; mat[3][2] -= a[3][2]; mat[3][3] -= a[3][3]; mat[3][4] -= a[3][4]; mat[3][5] -= a[3][5];
	mat[4][0] -= a[4][0]; mat[4][1] -= a[4][1]; mat[4][2] -= a[4][2]; mat[4][3] -= a[4][3]; mat[4][4] -= a[4][4]; mat[4][5] -= a[4][5];
	mat[5][0] -= a[5][0]; mat[5][1] -= a[5][1]; mat[5][2] -= a[5][2]; mat[5][3] -= a[5][3]; mat[5][4] -= a[5][4]; mat[5][5] -= a[5][5];
	return *this;
}

Z_INLINE CVec6 operator*( const CVec6 &vec, const CMat6 &mat ) {
	return mat * vec;
}

Z_INLINE CMat6 operator*( const float a, CMat6 const &mat ) {
	return mat * a;
}

Z_INLINE CVec6 &operator*=( CVec6 &vec, const CMat6 &mat ) {
	vec = mat * vec;
	return vec;
}

Z_INLINE bool CMat6::compare( const CMat6 &a ) const {
	DWORD i;
	const float *ptr1, *ptr2;

	ptr1 = reinterpret_cast<const float *>(mat);
	ptr2 = reinterpret_cast<const float *>(a.mat);
	for ( i = 0; i < 6*6; i++ ) {
		if ( ptr1[i] != ptr2[i] ) {
			return false;
		}
	}
	return true;
}

Z_INLINE bool CMat6::compare( const CMat6 &a, const float epsilon ) const {
	DWORD i;
	const float *ptr1, *ptr2;

	ptr1 = reinterpret_cast<const float *>(mat);
	ptr2 = reinterpret_cast<const float *>(a.mat);
	for ( i = 0; i < 6*6; i++ ) {
		if ( CZMath::Fabs( ptr1[i] - ptr2[i] ) > epsilon ) {
			return false;
		}
	}
	return true;
}

Z_INLINE bool CMat6::operator==( const CMat6 &a ) const {
	return compare( a );
}

Z_INLINE bool CMat6::operator!=( const CMat6 &a ) const {
	return !compare( a );
}

Z_INLINE void CMat6::zero( void ) {
	memset( mat, 0, sizeof( CMat6 ) );
}

Z_INLINE void CMat6::identity( void ) {
	*this = mat6_identity;
}

Z_INLINE bool CMat6::isidentity( const float epsilon ) const {
	return compare( mat6_identity, epsilon );
}

Z_INLINE bool CMat6::isSymmetric( const float epsilon ) const {
	for ( int i = 1; i < 6; i++ ) {
		for ( int j = 0; j < i; j++ ) {
			if ( CZMath::Fabs( mat[i][j] - mat[j][i] ) > epsilon ) {
				return false;
			}
		}
	}
	return true;
}

Z_INLINE bool CMat6::isDiagonal( const float epsilon ) const {
	for ( int i = 0; i < 6; i++ ) {
		for ( int j = 0; j < 6; j++ ) {
			if ( i != j && CZMath::Fabs( mat[i][j] ) > epsilon ) {
				return false;
			}
		}
	}
	return true;
}

Z_INLINE CMat3 CMat6::subMat3( int n ) const {
	assert( n >= 0 && n < 4 );
	int b0 = ((n & 2) >> 1) * 3;
	int b1 = (n & 1) * 3;
	return CMat3(
		mat[b0 + 0][b1 + 0], mat[b0 + 0][b1 + 1], mat[b0 + 0][b1 + 2],
		mat[b0 + 1][b1 + 0], mat[b0 + 1][b1 + 1], mat[b0 + 1][b1 + 2],
		mat[b0 + 2][b1 + 0], mat[b0 + 2][b1 + 1], mat[b0 + 2][b1 + 2] );
}

Z_INLINE float CMat6::trace( void ) const {
	return ( mat[0][0] + mat[1][1] + mat[2][2] + mat[3][3] + mat[4][4] + mat[5][5] );
}

Z_INLINE CMat6 CMat6::inverse( void ) const {
	CMat6 invMat;

	invMat = *this;
	int r = invMat.inverseSelf();
	assert( r );
	return invMat;
}

Z_INLINE CMat6 CMat6::inverseFast( void ) const {
	CMat6 invMat;

	invMat = *this;
	int r = invMat.inverseFastSelf();
	assert( r );
	return invMat;
}

Z_INLINE int CMat6::getDimension( void ) const {
	return 36;
}

Z_INLINE const float *CMat6::toFloatPtr( void ) const {
	return mat[0].toFloatPtr();
}

Z_INLINE float *CMat6::toFloatPtr( void ) {
	return mat[0].toFloatPtr();
}


//===============================================================
//
//	idMatX - arbitrary sized dense real matrix
//
//  The matrix lives on 16 byte aligned and 16 byte padded memory.
//
//	NOTE: due to the temporary memory pool idMatX cannot be used by multiple threads.
//
//===============================================================
/*
#define MATX_MAX_TEMP		1024
#define MATX_QUAD( x )		( ( ( ( x ) + 3 ) & ~3 ) * sizeof( float ) )
#define MATX_CLEAREND()		int s = numRows * numColumns; while( s < ( ( s + 3 ) & ~3 ) ) { mat[s++] = 0.0f; }
#define MATX_ALLOCA( n )	( (float *) _alloca16( MATX_QUAD( n ) ) )
#define MATX_SIMD

class idMatX {
public:
					idMatX( void );
					explicit idMatX( int rows, int columns );
					explicit idMatX( int rows, int columns, float *src );
					~idMatX( void );

	void			Set( int rows, int columns, const float *src );
	void			Set( const CMat3 &m1, const CMat3 &m2 );
	void			Set( const CMat3 &m1, const CMat3 &m2, const CMat3 &m3, const CMat3 &m4 );

	const float *	operator[]( int index ) const;
	float *			operator[]( int index );
	idMatX &		operator=( const idMatX &a );
	idMatX			operator*( const float a ) const;
	idVecX			operator*( const idVecX &vec ) const;
	idMatX			operator*( const idMatX &a ) const;
	idMatX			operator+( const idMatX &a ) const;
	idMatX			operator-( const idMatX &a ) const;
	idMatX &		operator*=( const float a );
	idMatX &		operator*=( const idMatX &a );
	idMatX &		operator+=( const idMatX &a );
	idMatX &		operator-=( const idMatX &a );

	friend idMatX	operator*( const float a, const idMatX &m );
	friend idVecX	operator*( const idVecX &vec, const idMatX &m );
	friend idVecX &	operator*=( idVecX &vec, const idMatX &m );

	bool			compare( const idMatX &a ) const;								// exact compare, no epsilon
	bool			compare( const idMatX &a, const float epsilon ) const;			// compare with epsilon
	bool			operator==( const idMatX &a ) const;							// exact compare, no epsilon
	bool			operator!=( const idMatX &a ) const;							// exact compare, no epsilon

	void			SetSize( int rows, int columns );								// set the number of rows/columns
	void			ChangeSize( int rows, int columns, bool makeZero = false );		// change the size keeping data intact where possible
	int				GetNumRows( void ) const { return numRows; }					// get the number of rows
	int				GetNumColumns( void ) const { return numColumns; }				// get the number of columns
	void			SetData( int rows, int columns, float *data );					// set float array pointer
	void			zero( void );													// clear matrix
	void			zero( int rows, int columns );									// set size and clear matrix
	void			identity( void );												// clear to identity matrix
	void			identity( int rows, int columns );								// set size and clear to identity matrix
	void			Diag( const idVecX &v );										// create diagonal matrix from vector
	void			Random( int seed, float l = 0.0f, float u = 1.0f );				// fill matrix with random values
	void			Random( int rows, int columns, int seed, float l = 0.0f, float u = 1.0f );
	void			Negate( void );													// (*this) = - (*this)
	void			Clamp( float min, float max );									// clamp all values
	idMatX &		SwapRows( int r1, int r2 );										// swap rows
	idMatX &		SwapColumns( int r1, int r2 );									// swap columns
	idMatX &		SwapRowsColumns( int r1, int r2 );								// swap rows and columns
	idMatX &		RemoveRow( int r );												// remove a row
	idMatX &		RemoveColumn( int r );											// remove a column
	idMatX &		RemoveRowColumn( int r );										// remove a row and column
	void			ClearUpperTriangle( void );										// clear the upper triangle
	void			ClearLowerTriangle( void );										// clear the lower triangle
	void			SquareSubMatrix( const idMatX &m, int size );					// get square sub-matrix from 0,0 to size,size
	float			MaxDifference( const idMatX &m ) const;							// return maximum element difference between this and m

	bool			IsSquare( void ) const { return ( numRows == numColumns ); }
	bool			Iszero( const float epsilon = MATRIX_EPSILON ) const;
	bool			isidentity( const float epsilon = MATRIX_EPSILON ) const;
	bool			isDiagonal( const float epsilon = MATRIX_EPSILON ) const;
	bool			IsTriDiagonal( const float epsilon = MATRIX_EPSILON ) const;
	bool			isSymmetric( const float epsilon = MATRIX_EPSILON ) const;
	bool			IsOrthogonal( const float epsilon = MATRIX_EPSILON ) const;
	bool			IsOrthonormal( const float epsilon = MATRIX_EPSILON ) const;
	bool			IsPMatrix( const float epsilon = MATRIX_EPSILON ) const;
	bool			IsZMatrix( const float epsilon = MATRIX_EPSILON ) const;
	bool			IsPositiveDefinite( const float epsilon = MATRIX_EPSILON ) const;
	bool			isSymmetricPositiveDefinite( const float epsilon = MATRIX_EPSILON ) const;
	bool			IsPositiveSemiDefinite( const float epsilon = MATRIX_EPSILON ) const;
	bool			isSymmetricPositiveSemiDefinite( const float epsilon = MATRIX_EPSILON ) const;

	float			trace( void ) const;											// returns product of diagonal elements
	float			determinant( void ) const;										// returns determinant of matrix
	idMatX			transpose( void ) const;										// returns transpose
	idMatX &		transposeSelf( void );											// transposes the matrix itself
	idMatX			inverse( void ) const;											// returns the inverse ( m * m.inverse() = identity )
	bool			inverseSelf( void );											// returns false if determinant is zero
	idMatX			inverseFast( void ) const;										// returns the inverse ( m * m.inverse() = identity )
	bool			inverseFastSelf( void );										// returns false if determinant is zero

	bool			LowerTriangularinverse( void );									// in-place inversion, returns false if determinant is zero
	bool			UpperTriangularinverse( void );									// in-place inversion, returns false if determinant is zero

	idVecX			Multiply( const idVecX &vec ) const;							// (*this) * vec
	idVecX			transposeMultiply( const idVecX &vec ) const;					// this->transpose() * vec

	idMatX			Multiply( const idMatX &a ) const;								// (*this) * a
	idMatX			transposeMultiply( const idMatX &a ) const;						// this->transpose() * a

	void			Multiply( idVecX &dst, const idVecX &vec ) const;				// dst = (*this) * vec
	void			MultiplyAdd( idVecX &dst, const idVecX &vec ) const;			// dst += (*this) * vec
	void			MultiplySub( idVecX &dst, const idVecX &vec ) const;			// dst -= (*this) * vec
	void			transposeMultiply( idVecX &dst, const idVecX &vec ) const;		// dst = this->transpose() * vec
	void			transposeMultiplyAdd( idVecX &dst, const idVecX &vec ) const;	// dst += this->transpose() * vec
	void			transposeMultiplySub( idVecX &dst, const idVecX &vec ) const;	// dst -= this->transpose() * vec

	void			Multiply( idMatX &dst, const idMatX &a ) const;					// dst = (*this) * a
	void			transposeMultiply( idMatX &dst, const idMatX &a ) const;		// dst = this->transpose() * a

	int				getDimension( void ) const;										// returns total number of values in matrix

	const CVec6 &	SubVec6( int row ) const;										// interpret beginning of row as a const CVec6
	CVec6 &		SubVec6( int row );												// interpret beginning of row as an CVec6
	const idVecX	SubVecX( int row ) const;										// interpret complete row as a const idVecX
	idVecX			SubVecX( int row );												// interpret complete row as an idVecX
	const float *	toFloatPtr( void ) const;										// pointer to const matrix float array
	float *			toFloatPtr( void );												// pointer to matrix float array
	const char *	toString( int precision = 2 ) const;

	void			Update_RankOne( const idVecX &v, const idVecX &w, float alpha );
	void			Update_RankOneSymmetric( const idVecX &v, float alpha );
	void			Update_RowColumn( const idVecX &v, const idVecX &w, int r );
	void			Update_RowColumnSymmetric( const idVecX &v, int r );
	void			Update_Increment( const idVecX &v, const idVecX &w );
	void			Update_IncrementSymmetric( const idVecX &v );
	void			Update_Decrement( int r );

	bool			inverse_GaussJordan( void );					// invert in-place with Gauss-Jordan elimination
	bool			inverse_UpdateRankOne( const idVecX &v, const idVecX &w, float alpha );
	bool			inverse_UpdateRowColumn( const idVecX &v, const idVecX &w, int r );
	bool			inverse_UpdateIncrement( const idVecX &v, const idVecX &w );
	bool			inverse_UpdateDecrement( const idVecX &v, const idVecX &w, int r );
	void			inverse_Solve( idVecX &x, const idVecX &b ) const;

	bool			LU_Factor( int *index, float *det = NULL );		// factor in-place: L * U
	bool			LU_UpdateRankOne( const idVecX &v, const idVecX &w, float alpha, int *index );
	bool			LU_UpdateRowColumn( const idVecX &v, const idVecX &w, int r, int *index );
	bool			LU_UpdateIncrement( const idVecX &v, const idVecX &w, int *index );
	bool			LU_UpdateDecrement( const idVecX &v, const idVecX &w, const idVecX &u, int r, int *index );
	void			LU_Solve( idVecX &x, const idVecX &b, const int *index ) const;
	void			LU_inverse( idMatX &inv, const int *index ) const;
	void			LU_UnpackFactors( idMatX &L, idMatX &U ) const;
	void			LU_MultiplyFactors( idMatX &m, const int *index ) const;

	bool			QR_Factor( idVecX &c, idVecX &d );				// factor in-place: Q * R
	bool			QR_UpdateRankOne( idMatX &R, const idVecX &v, const idVecX &w, float alpha );
	bool			QR_UpdateRowColumn( idMatX &R, const idVecX &v, const idVecX &w, int r );
	bool			QR_UpdateIncrement( idMatX &R, const idVecX &v, const idVecX &w );
	bool			QR_UpdateDecrement( idMatX &R, const idVecX &v, const idVecX &w, int r );
	void			QR_Solve( idVecX &x, const idVecX &b, const idVecX &c, const idVecX &d ) const;
	void			QR_Solve( idVecX &x, const idVecX &b, const idMatX &R ) const;
	void			QR_inverse( idMatX &inv, const idVecX &c, const idVecX &d ) const;
	void			QR_UnpackFactors( idMatX &Q, idMatX &R, const idVecX &c, const idVecX &d ) const;
	void			QR_MultiplyFactors( idMatX &m, const idVecX &c, const idVecX &d ) const;

	bool			SVD_Factor( idVecX &w, idMatX &V );				// factor in-place: U * Diag(w) * V.transpose()
	void			SVD_Solve( idVecX &x, const idVecX &b, const idVecX &w, const idMatX &V ) const;
	void			SVD_inverse( idMatX &inv, const idVecX &w, const idMatX &V ) const;
	void			SVD_MultiplyFactors( idMatX &m, const idVecX &w, const idMatX &V ) const;

	bool			Cholesky_Factor( void );						// factor in-place: L * L.transpose()
	bool			Cholesky_UpdateRankOne( const idVecX &v, float alpha, int offset = 0 );
	bool			Cholesky_UpdateRowColumn( const idVecX &v, int r );
	bool			Cholesky_UpdateIncrement( const idVecX &v );
	bool			Cholesky_UpdateDecrement( const idVecX &v, int r );
	void			Cholesky_Solve( idVecX &x, const idVecX &b ) const;
	void			Cholesky_inverse( idMatX &inv ) const;
	void			Cholesky_MultiplyFactors( idMatX &m ) const;

	bool			LDLT_Factor( void );							// factor in-place: L * D * L.transpose()
	bool			LDLT_UpdateRankOne( const idVecX &v, float alpha, int offset = 0 );
	bool			LDLT_UpdateRowColumn( const idVecX &v, int r );
	bool			LDLT_UpdateIncrement( const idVecX &v );
	bool			LDLT_UpdateDecrement( const idVecX &v, int r );
	void			LDLT_Solve( idVecX &x, const idVecX &b ) const;
	void			LDLT_inverse( idMatX &inv ) const;
	void			LDLT_UnpackFactors( idMatX &L, idMatX &D ) const;
	void			LDLT_MultiplyFactors( idMatX &m ) const;

	void			TriDiagonal_ClearTriangles( void );
	bool			TriDiagonal_Solve( idVecX &x, const idVecX &b ) const;
	void			TriDiagonal_inverse( idMatX &inv ) const;

	bool			Eigen_SolveSymmetricTriDiagonal( idVecX &eigenValues );
	bool			Eigen_SolveSymmetric( idVecX &eigenValues );
	bool			Eigen_Solve( idVecX &realEigenValues, idVecX &imaginaryEigenValues );
	void			Eigen_SortIncreasing( idVecX &eigenValues );
	void			Eigen_SortDecreasing( idVecX &eigenValues );

	static void		Test( void );

private:
	int				numRows;				// number of rows
	int				numColumns;				// number of columns
	int				alloced;				// floats allocated, if -1 then mat points to data set with SetData
	float *			mat;					// memory the matrix is stored

	static float	temp[MATX_MAX_TEMP+4];	// used to store intermediate results
	static float *	tempPtr;				// pointer to 16 byte aligned temporary memory
	static int		tempIndex;				// index into memory pool, wraps around

private:
	void			SetTempSize( int rows, int columns );
	float			determinantGeneric( void ) const;
	bool			inverseSelfGeneric( void );
	void			QR_Rotate( idMatX &R, int i, float a, float b );
	float			Pythag( float a, float b ) const;
	void			SVD_BiDiag( idVecX &w, idVecX &rv1, float &anorm );
	void			SVD_InitialWV( idVecX &w, idMatX &V, idVecX &rv1 );
	void			HouseholderReduction( idVecX &diag, idVecX &subd );
	bool			QL( idVecX &diag, idVecX &subd );
	void			HessenbergReduction( idMatX &H );
	void			ComplexDivision( float xr, float xi, float yr, float yi, float &cdivr, float &cdivi );
	bool			HessenbergToRealSchur( idMatX &H, idVecX &realEigenValues, idVecX &imaginaryEigenValues );
};

Z_INLINE idMatX::idMatX( void ) {
	numRows = numColumns = alloced = 0;
	mat = NULL;
}

Z_INLINE idMatX::~idMatX( void ) {
	// if not temp memory
	if ( mat != NULL && ( mat < idMatX::tempPtr || mat > idMatX::tempPtr + MATX_MAX_TEMP ) && alloced != -1 ) {
		Mem_Free16( mat );
	}
}

Z_INLINE idMatX::idMatX( int rows, int columns ) {
	numRows = numColumns = alloced = 0;
	mat = NULL;
	SetSize( rows, columns );
}

Z_INLINE idMatX::idMatX( int rows, int columns, float *src ) {
	numRows = numColumns = alloced = 0;
	mat = NULL;
	SetData( rows, columns, src );
}

Z_INLINE void idMatX::Set( int rows, int columns, const float *src ) {
	SetSize( rows, columns );
	memcpy( this->mat, src, rows * columns * sizeof( float ) );
}

Z_INLINE void idMatX::Set( const CMat3 &m1, const CMat3 &m2 ) {
	int i, j;

	SetSize( 3, 6 );
	for ( i = 0; i < 3; i++ ) {
		for ( j = 0; j < 3; j++ ) {
			mat[(i+0) * numColumns + (j+0)] = m1[i][j];
			mat[(i+0) * numColumns + (j+3)] = m2[i][j];
		}
	}
}

Z_INLINE void idMatX::Set( const CMat3 &m1, const CMat3 &m2, const CMat3 &m3, const CMat3 &m4 ) {
	int i, j;

	SetSize( 6, 6 );
	for ( i = 0; i < 3; i++ ) {
		for ( j = 0; j < 3; j++ ) {
			mat[(i+0) * numColumns + (j+0)] = m1[i][j];
			mat[(i+0) * numColumns + (j+3)] = m2[i][j];
			mat[(i+3) * numColumns + (j+0)] = m3[i][j];
			mat[(i+3) * numColumns + (j+3)] = m4[i][j];
		}
	}
}

Z_INLINE const float *idMatX::operator[]( int index ) const {
	assert( ( index >= 0 ) && ( index < numRows ) );
	return mat + index * numColumns;
}

Z_INLINE float *idMatX::operator[]( int index ) {
	assert( ( index >= 0 ) && ( index < numRows ) );
	return mat + index * numColumns;
}

Z_INLINE idMatX &idMatX::operator=( const idMatX &a ) {
	SetSize( a.numRows, a.numColumns );
#ifdef MATX_SIMD
	SIMDProcessor->Copy16( mat, a.mat, a.numRows * a.numColumns );
#else
	memcpy( mat, a.mat, a.numRows * a.numColumns * sizeof( float ) );
#endif
	idMatX::tempIndex = 0;
	return *this;
}

Z_INLINE idMatX idMatX::operator*( const float a ) const {
	idMatX m;

	m.SetTempSize( numRows, numColumns );
#ifdef MATX_SIMD
	SIMDProcessor->Mul16( m.mat, mat, a, numRows * numColumns );
#else
	int i, s;
	s = numRows * numColumns;
	for ( i = 0; i < s; i++ ) {
		m.mat[i] = mat[i] * a;
	}
#endif
	return m;
}

Z_INLINE idVecX idMatX::operator*( const idVecX &vec ) const {
	idVecX dst;

	assert( numColumns == vec.GetSize() );

	dst.SetTempSize( numRows );
#ifdef MATX_SIMD
	SIMDProcessor->MatX_MultiplyVecX( dst, *this, vec );
#else
	Multiply( dst, vec );
#endif
	return dst;
}

Z_INLINE idMatX idMatX::operator*( const idMatX &a ) const {
	idMatX dst;

	assert( numColumns == a.numRows );

	dst.SetTempSize( numRows, a.numColumns );
#ifdef MATX_SIMD
	SIMDProcessor->MatX_MultiplyMatX( dst, *this, a );
#else
	Multiply( dst, a );
#endif
	return dst;
}

Z_INLINE idMatX idMatX::operator+( const idMatX &a ) const {
	idMatX m;

	assert( numRows == a.numRows && numColumns == a.numColumns );
	m.SetTempSize( numRows, numColumns );
#ifdef MATX_SIMD
	SIMDProcessor->Add16( m.mat, mat, a.mat, numRows * numColumns );
#else
	int i, s;
	s = numRows * numColumns;
	for ( i = 0; i < s; i++ ) {
		m.mat[i] = mat[i] + a.mat[i];
	}
#endif
	return m;
}

Z_INLINE idMatX idMatX::operator-( const idMatX &a ) const {
	idMatX m;

	assert( numRows == a.numRows && numColumns == a.numColumns );
	m.SetTempSize( numRows, numColumns );
#ifdef MATX_SIMD
	SIMDProcessor->Sub16( m.mat, mat, a.mat, numRows * numColumns );
#else
	int i, s;
	s = numRows * numColumns;
	for ( i = 0; i < s; i++ ) {
		m.mat[i] = mat[i] - a.mat[i];
	}
#endif
	return m;
}

Z_INLINE idMatX &idMatX::operator*=( const float a ) {
#ifdef MATX_SIMD
	SIMDProcessor->MulAssign16( mat, a, numRows * numColumns );
#else
	int i, s;
	s = numRows * numColumns;
	for ( i = 0; i < s; i++ ) {
		mat[i] *= a;
	}
#endif
	idMatX::tempIndex = 0;
	return *this;
}

Z_INLINE idMatX &idMatX::operator*=( const idMatX &a ) {
	*this = *this * a;
	idMatX::tempIndex = 0;
	return *this;
}

Z_INLINE idMatX &idMatX::operator+=( const idMatX &a ) {
	assert( numRows == a.numRows && numColumns == a.numColumns );
#ifdef MATX_SIMD
	SIMDProcessor->AddAssign16( mat, a.mat, numRows * numColumns );
#else
	int i, s;
	s = numRows * numColumns;
	for ( i = 0; i < s; i++ ) {
		mat[i] += a.mat[i];
	}
#endif
	idMatX::tempIndex = 0;
	return *this;
}

Z_INLINE idMatX &idMatX::operator-=( const idMatX &a ) {
	assert( numRows == a.numRows && numColumns == a.numColumns );
#ifdef MATX_SIMD
	SIMDProcessor->SubAssign16( mat, a.mat, numRows * numColumns );
#else
	int i, s;
	s = numRows * numColumns;
	for ( i = 0; i < s; i++ ) {
		mat[i] -= a.mat[i];
	}
#endif
	idMatX::tempIndex = 0;
	return *this;
}

Z_INLINE idMatX operator*( const float a, idMatX const &m ) {
	return m * a;
}

Z_INLINE idVecX operator*( const idVecX &vec, const idMatX &m ) {
	return m * vec;
}

Z_INLINE idVecX &operator*=( idVecX &vec, const idMatX &m ) {
	vec = m * vec;
	return vec;
}

Z_INLINE bool idMatX::compare( const idMatX &a ) const {
	int i, s;

	assert( numRows == a.numRows && numColumns == a.numColumns );

	s = numRows * numColumns;
	for ( i = 0; i < s; i++ ) {
		if ( mat[i] != a.mat[i] ) {
			return false;
		}
	}
	return true;
}

Z_INLINE bool idMatX::compare( const idMatX &a, const float epsilon ) const {
	int i, s;

	assert( numRows == a.numRows && numColumns == a.numColumns );

	s = numRows * numColumns;
	for ( i = 0; i < s; i++ ) {
		if ( CZMath::Fabs( mat[i] - a.mat[i] ) > epsilon ) {
			return false;
		}
	}
	return true;
}

Z_INLINE bool idMatX::operator==( const idMatX &a ) const {
	return compare( a );
}

Z_INLINE bool idMatX::operator!=( const idMatX &a ) const {
	return !compare( a );
}

Z_INLINE void idMatX::SetSize( int rows, int columns ) {
	assert( mat < idMatX::tempPtr || mat > idMatX::tempPtr + MATX_MAX_TEMP );
	int alloc = ( rows * columns + 3 ) & ~3;
	if ( alloc > alloced && alloced != -1 ) {
		if ( mat != NULL ) {
			Mem_Free16( mat );
		}
		mat = (float *) Mem_Alloc16( alloc * sizeof( float ) );
		alloced = alloc;
	}
	numRows = rows;
	numColumns = columns;
	MATX_CLEAREND();
}

Z_INLINE void idMatX::SetTempSize( int rows, int columns ) {
	int newSize;

	newSize = ( rows * columns + 3 ) & ~3;
	assert( newSize < MATX_MAX_TEMP );
	if ( idMatX::tempIndex + newSize > MATX_MAX_TEMP ) {
		idMatX::tempIndex = 0;
	}
	mat = idMatX::tempPtr + idMatX::tempIndex;
	idMatX::tempIndex += newSize;
	alloced = newSize;
	numRows = rows;
	numColumns = columns;
	MATX_CLEAREND();
}

Z_INLINE void idMatX::SetData( int rows, int columns, float *data ) {
	assert( mat < idMatX::tempPtr || mat > idMatX::tempPtr + MATX_MAX_TEMP );
	if ( mat != NULL && alloced != -1 ) {
		Mem_Free16( mat );
	}
	assert( ( ( (int) data ) & 15 ) == 0 ); // data must be 16 byte aligned
	mat = data;
	alloced = -1;
	numRows = rows;
	numColumns = columns;
	MATX_CLEAREND();
}

Z_INLINE void idMatX::zero( void ) {
#ifdef MATX_SIMD
	SIMDProcessor->Zero16( mat, numRows * numColumns );
#else
	memset( mat, 0, numRows * numColumns * sizeof( float ) );
#endif
}

Z_INLINE void idMatX::zero( int rows, int columns ) {
	SetSize( rows, columns );
#ifdef MATX_SIMD
	SIMDProcessor->Zero16( mat, numRows * numColumns );
#else
	memset( mat, 0, rows * columns * sizeof( float ) );
#endif
}

Z_INLINE void idMatX::identity( void ) {
	assert( numRows == numColumns );
#ifdef MATX_SIMD
	SIMDProcessor->Zero16( mat, numRows * numColumns );
#else
	memset( mat, 0, numRows * numColumns * sizeof( float ) );
#endif
	for ( int i = 0; i < numRows; i++ ) {
		mat[i * numColumns + i] = 1.0f;
	}
}

Z_INLINE void idMatX::identity( int rows, int columns ) {
	assert( rows == columns );
	SetSize( rows, columns );
	idMatX::identity();
}

Z_INLINE void idMatX::Diag( const idVecX &v ) {
	zero( v.GetSize(), v.GetSize() );
	for ( int i = 0; i < v.GetSize(); i++ ) {
		mat[i * numColumns + i] = v[i];
	}
}

Z_INLINE void idMatX::Random( int seed, float l, float u ) {
	int i, s;
	float c;
	idRandom rnd(seed);

	c = u - l;
	s = numRows * numColumns;
	for ( i = 0; i < s; i++ ) {
		mat[i] = l + rnd.RandomFloat() * c;
	}
}

Z_INLINE void idMatX::Random( int rows, int columns, int seed, float l, float u ) {
	int i, s;
	float c;
	idRandom rnd(seed);

	SetSize( rows, columns );
	c = u - l;
	s = numRows * numColumns;
	for ( i = 0; i < s; i++ ) {
		mat[i] = l + rnd.RandomFloat() * c;
	}
}

Z_INLINE void idMatX::Negate( void ) {
#ifdef MATX_SIMD
	SIMDProcessor->Negate16( mat, numRows * numColumns );
#else
	int i, s;
	s = numRows * numColumns;
	for ( i = 0; i < s; i++ ) {
		mat[i] = -mat[i];
	}
#endif
}

Z_INLINE void idMatX::Clamp( float min, float max ) {
	int i, s;
	s = numRows * numColumns;
	for ( i = 0; i < s; i++ ) {
		if ( mat[i] < min ) {
			mat[i] = min;
		} else if ( mat[i] > max ) {
			mat[i] = max;
		}
	}
}

Z_INLINE idMatX &idMatX::SwapRows( int r1, int r2 ) {
	float *ptr;

	ptr = (float *) _alloca16( numColumns * sizeof( float ) );
	memcpy( ptr, mat + r1 * numColumns, numColumns * sizeof( float ) );
	memcpy( mat + r1 * numColumns, mat + r2 * numColumns, numColumns * sizeof( float ) );
	memcpy( mat + r2 * numColumns, ptr, numColumns * sizeof( float ) );

	return *this;
}

Z_INLINE idMatX &idMatX::SwapColumns( int r1, int r2 ) {
	int i;
	float tmp, *ptr;

	for ( i = 0; i < numRows; i++ ) {
		ptr = mat + i * numColumns;
		tmp = ptr[r1];
		ptr[r1] = ptr[r2];
		ptr[r2] = tmp;
	}

	return *this;
}

Z_INLINE idMatX &idMatX::SwapRowsColumns( int r1, int r2 ) {

	SwapRows( r1, r2 );
	SwapColumns( r1, r2 );
	return *this;
}

Z_INLINE void idMatX::ClearUpperTriangle( void ) {
	assert( numRows == numColumns );
	for ( int i = numRows-2; i >= 0; i-- ) {
		memset( mat + i * numColumns + i + 1, 0, (numColumns - 1 - i) * sizeof(float) );
	}
}

Z_INLINE void idMatX::ClearLowerTriangle( void ) {
	assert( numRows == numColumns );
	for ( int i = 1; i < numRows; i++ ) {
		memset( mat + i * numColumns, 0, i * sizeof(float) );
	}
}

Z_INLINE void idMatX::SquareSubMatrix( const idMatX &m, int size ) {
	int i;
	assert( size <= m.numRows && size <= m.numColumns );
	SetSize( size, size );
	for ( i = 0; i < size; i++ ) {
		memcpy( mat + i * numColumns, m.mat + i * m.numColumns, size * sizeof( float ) );
	}
}

Z_INLINE float idMatX::MaxDifference( const idMatX &m ) const {
	int i, j;
	float diff, maxDiff;

	assert( numRows == m.numRows && numColumns == m.numColumns );

	maxDiff = -1.0f;
	for ( i = 0; i < numRows; i++ ) {
		for ( j = 0; j < numColumns; j++ ) {
			diff = CZMath::Fabs( mat[ i * numColumns + j ] - m[i][j] );
			if ( maxDiff < 0.0f || diff > maxDiff ) {
				maxDiff = diff;
			}
		}
	}
	return maxDiff;
}

Z_INLINE bool idMatX::Iszero( const float epsilon ) const {
	// returns true if (*this) == Zero
	for ( int i = 0; i < numRows; i++ ) {
		for ( int j = 0; j < numColumns; j++ ) {
			if ( CZMath::Fabs( mat[i * numColumns + j] ) > epsilon ) {
				return false;
			}
		}
	}
	return true;
}

Z_INLINE bool idMatX::isidentity( const float epsilon ) const {
	// returns true if (*this) == Identity
	assert( numRows == numColumns );
	for ( int i = 0; i < numRows; i++ ) {
		for ( int j = 0; j < numColumns; j++ ) {
			if ( CZMath::Fabs( mat[i * numColumns + j] - (float)( i == j ) ) > epsilon ) {
				return false;
			}
		}
	}
	return true;
}

Z_INLINE bool idMatX::isDiagonal( const float epsilon ) const {
	// returns true if all elements are zero except for the elements on the diagonal
	assert( numRows == numColumns );
	for ( int i = 0; i < numRows; i++ ) {
		for ( int j = 0; j < numColumns; j++ ) {
			if ( i != j && CZMath::Fabs( mat[i * numColumns + j] ) > epsilon ) {
				return false;
			}
		}
	}
	return true;
}

Z_INLINE bool idMatX::IsTriDiagonal( const float epsilon ) const {
	// returns true if all elements are zero except for the elements on the diagonal plus or minus one column

	if ( numRows != numColumns ) {
		return false;
	}
	for ( int i = 0; i < numRows-2; i++ ) {
		for ( int j = i+2; j < numColumns; j++ ) {
			if ( CZMath::Fabs( (*this)[i][j] ) > epsilon ) {
				return false;
			}
			if ( CZMath::Fabs( (*this)[j][i] ) > epsilon ) {
				return false;
			}
		}
	}
	return true;
}

Z_INLINE bool idMatX::isSymmetric( const float epsilon ) const {
	// (*this)[i][j] == (*this)[j][i]
	if ( numRows != numColumns ) {
		return false;
	}
	for ( int i = 0; i < numRows; i++ ) {
		for ( int j = 0; j < numColumns; j++ ) {
			if ( CZMath::Fabs( mat[ i * numColumns + j ] - mat[ j * numColumns + i ] ) > epsilon ) {
				return false;
			}
		}
	}
	return true;
}

Z_INLINE float idMatX::trace( void ) const {
	float trace = 0.0f;

	assert( numRows == numColumns );

	// sum of elements on the diagonal
	for ( int i = 0; i < numRows; i++ ) {
		trace += mat[i * numRows + i];
	}
	return trace;
}

Z_INLINE float idMatX::determinant( void ) const {

	assert( numRows == numColumns );

	switch( numRows ) {
		case 1:
			return mat[0];
		case 2:
			return reinterpret_cast<const CMat2 *>(mat)->determinant();
		case 3:
			return reinterpret_cast<const CMat3 *>(mat)->determinant();
		case 4:
			return reinterpret_cast<const CMat4 *>(mat)->determinant();
		case 5:
			return reinterpret_cast<const CMat5 *>(mat)->determinant();
		case 6:
			return reinterpret_cast<const CMat6 *>(mat)->determinant();
		default:
			return determinantGeneric();
	}
	return 0.0f;
}

Z_INLINE idMatX idMatX::transpose( void ) const {
	idMatX transpose;
	int i, j;

	transpose.SetTempSize( numColumns, numRows );

	for ( i = 0; i < numRows; i++ ) {
		for ( j = 0; j < numColumns; j++ ) {
			transpose.mat[j * transpose.numColumns + i] = mat[i * numColumns + j];
		}
	}

	return transpose;
}

Z_INLINE idMatX &idMatX::transposeSelf( void ) {
	*this = transpose();
	return *this;
}

Z_INLINE idMatX idMatX::inverse( void ) const {
	idMatX invMat;

	invMat.SetTempSize( numRows, numColumns );
	memcpy( invMat.mat, mat, numRows * numColumns * sizeof( float ) );
	int r = invMat.inverseSelf();
	assert( r );
	return invMat;
}

Z_INLINE bool idMatX::inverseSelf( void ) {

	assert( numRows == numColumns );

	switch( numRows ) {
		case 1:
			if ( CZMath::Fabs( mat[0] ) < MATRIX_INVERSE_EPSILON ) {
				return false;
			}
			mat[0] = 1.0f / mat[0];
			return true;
		case 2:
			return reinterpret_cast<CMat2 *>(mat)->inverseSelf();
		case 3:
			return reinterpret_cast<CMat3 *>(mat)->inverseSelf();
		case 4:
			return reinterpret_cast<CMat4 *>(mat)->inverseSelf();
		case 5:
			return reinterpret_cast<CMat5 *>(mat)->inverseSelf();
		case 6:
			return reinterpret_cast<CMat6 *>(mat)->inverseSelf();
		default:
			return inverseSelfGeneric();
	}
	return false;
}

Z_INLINE idMatX idMatX::inverseFast( void ) const {
	idMatX invMat;

	invMat.SetTempSize( numRows, numColumns );
	memcpy( invMat.mat, mat, numRows * numColumns * sizeof( float ) );
	int r = invMat.inverseFastSelf();
	assert( r );
	return invMat;
}

Z_INLINE bool idMatX::inverseFastSelf( void ) {

	assert( numRows == numColumns );

	switch( numRows ) {
		case 1:
			if ( CZMath::Fabs( mat[0] ) < MATRIX_INVERSE_EPSILON ) {
				return false;
			}
			mat[0] = 1.0f / mat[0];
			return true;
		case 2:
			return reinterpret_cast<CMat2 *>(mat)->inverseFastSelf();
		case 3:
			return reinterpret_cast<CMat3 *>(mat)->inverseFastSelf();
		case 4:
			return reinterpret_cast<CMat4 *>(mat)->inverseFastSelf();
		case 5:
			return reinterpret_cast<CMat5 *>(mat)->inverseFastSelf();
		case 6:
			return reinterpret_cast<CMat6 *>(mat)->inverseFastSelf();
		default:
			return inverseSelfGeneric();
	}
	return false;
}

Z_INLINE idVecX idMatX::Multiply( const idVecX &vec ) const {
	idVecX dst;

	assert( numColumns == vec.GetSize() );

	dst.SetTempSize( numRows );
#ifdef MATX_SIMD
	SIMDProcessor->MatX_MultiplyVecX( dst, *this, vec );
#else
	Multiply( dst, vec );
#endif
	return dst;
}

Z_INLINE idMatX idMatX::Multiply( const idMatX &a ) const {
	idMatX dst;

	assert( numColumns == a.numRows );

	dst.SetTempSize( numRows, a.numColumns );
#ifdef MATX_SIMD
	SIMDProcessor->MatX_MultiplyMatX( dst, *this, a );
#else
	Multiply( dst, a );
#endif
	return dst;
}

Z_INLINE idVecX idMatX::transposeMultiply( const idVecX &vec ) const {
	idVecX dst;

	assert( numRows == vec.GetSize() );

	dst.SetTempSize( numColumns );
#ifdef MATX_SIMD
	SIMDProcessor->MatX_transposeMultiplyVecX( dst, *this, vec );
#else
	transposeMultiply( dst, vec );
#endif
	return dst;
}

Z_INLINE idMatX idMatX::transposeMultiply( const idMatX &a ) const {
	idMatX dst;

	assert( numRows == a.numRows );

	dst.SetTempSize( numColumns, a.numColumns );
#ifdef MATX_SIMD
	SIMDProcessor->MatX_transposeMultiplyMatX( dst, *this, a );
#else
	transposeMultiply( dst, a );
#endif
	return dst;
}

Z_INLINE void idMatX::Multiply( idVecX &dst, const idVecX &vec ) const {
#ifdef MATX_SIMD
	SIMDProcessor->MatX_MultiplyVecX( dst, *this, vec );
#else
	int i, j;
	const float *mPtr, *vPtr;
	float *dstPtr;

	mPtr = mat;
	vPtr = vec.toFloatPtr();
	dstPtr = dst.toFloatPtr();
	for ( i = 0; i < numRows; i++ ) {
		float sum = mPtr[0] * vPtr[0];
		for ( j = 1; j < numColumns; j++ ) {
			sum += mPtr[j] * vPtr[j];
		}
		dstPtr[i] = sum;
		mPtr += numColumns;
	}
#endif
}

Z_INLINE void idMatX::MultiplyAdd( idVecX &dst, const idVecX &vec ) const {
#ifdef MATX_SIMD
	SIMDProcessor->MatX_MultiplyAddVecX( dst, *this, vec );
#else
	int i, j;
	const float *mPtr, *vPtr;
	float *dstPtr;

	mPtr = mat;
	vPtr = vec.toFloatPtr();
	dstPtr = dst.toFloatPtr();
	for ( i = 0; i < numRows; i++ ) {
		float sum = mPtr[0] * vPtr[0];
		for ( j = 1; j < numColumns; j++ ) {
			sum += mPtr[j] * vPtr[j];
		}
		dstPtr[i] += sum;
		mPtr += numColumns;
	}
#endif
}

Z_INLINE void idMatX::MultiplySub( idVecX &dst, const idVecX &vec ) const {
#ifdef MATX_SIMD
	SIMDProcessor->MatX_MultiplySubVecX( dst, *this, vec );
#else
	int i, j;
	const float *mPtr, *vPtr;
	float *dstPtr;

	mPtr = mat;
	vPtr = vec.toFloatPtr();
	dstPtr = dst.toFloatPtr();
	for ( i = 0; i < numRows; i++ ) {
		float sum = mPtr[0] * vPtr[0];
		for ( j = 1; j < numColumns; j++ ) {
			sum += mPtr[j] * vPtr[j];
		}
		dstPtr[i] -= sum;
		mPtr += numColumns;
	}
#endif
}

Z_INLINE void idMatX::transposeMultiply( idVecX &dst, const idVecX &vec ) const {
#ifdef MATX_SIMD
	SIMDProcessor->MatX_transposeMultiplyVecX( dst, *this, vec );
#else
	int i, j;
	const float *mPtr, *vPtr;
	float *dstPtr;

	vPtr = vec.toFloatPtr();
	dstPtr = dst.toFloatPtr();
	for ( i = 0; i < numColumns; i++ ) {
		mPtr = mat + i;
		float sum = mPtr[0] * vPtr[0];
		for ( j = 1; j < numRows; j++ ) {
			mPtr += numColumns;
			sum += mPtr[0] * vPtr[j];
		}
		dstPtr[i] = sum;
	}
#endif
}

Z_INLINE void idMatX::transposeMultiplyAdd( idVecX &dst, const idVecX &vec ) const {
#ifdef MATX_SIMD
	SIMDProcessor->MatX_transposeMultiplyAddVecX( dst, *this, vec );
#else
	int i, j;
	const float *mPtr, *vPtr;
	float *dstPtr;

	vPtr = vec.toFloatPtr();
	dstPtr = dst.toFloatPtr();
	for ( i = 0; i < numColumns; i++ ) {
		mPtr = mat + i;
		float sum = mPtr[0] * vPtr[0];
		for ( j = 1; j < numRows; j++ ) {
			mPtr += numColumns;
			sum += mPtr[0] * vPtr[j];
		}
		dstPtr[i] += sum;
	}
#endif
}

Z_INLINE void idMatX::transposeMultiplySub( idVecX &dst, const idVecX &vec ) const {
#ifdef MATX_SIMD
	SIMDProcessor->MatX_transposeMultiplySubVecX( dst, *this, vec );
#else
	int i, j;
	const float *mPtr, *vPtr;
	float *dstPtr;

	vPtr = vec.toFloatPtr();
	dstPtr = dst.toFloatPtr();
	for ( i = 0; i < numColumns; i++ ) {
		mPtr = mat + i;
		float sum = mPtr[0] * vPtr[0];
		for ( j = 1; j < numRows; j++ ) {
			mPtr += numColumns;
			sum += mPtr[0] * vPtr[j];
		}
		dstPtr[i] -= sum;
	}
#endif
}

Z_INLINE void idMatX::Multiply( idMatX &dst, const idMatX &a ) const {
#ifdef MATX_SIMD
	SIMDProcessor->MatX_MultiplyMatX( dst, *this, a );
#else
	int i, j, k, l, n;
	float *dstPtr;
	const float *m1Ptr, *m2Ptr;
	double sum;

	assert( numColumns == a.numRows );

	dstPtr = dst.toFloatPtr();
	m1Ptr = toFloatPtr();
	m2Ptr = a.toFloatPtr();
	k = numRows;
	l = a.GetNumColumns();

	for ( i = 0; i < k; i++ ) {
		for ( j = 0; j < l; j++ ) {
			m2Ptr = a.toFloatPtr() + j;
			sum = m1Ptr[0] * m2Ptr[0];
			for ( n = 1; n < numColumns; n++ ) {
				m2Ptr += l;
				sum += m1Ptr[n] * m2Ptr[0];
			}
			*dstPtr++ = sum;
		}
		m1Ptr += numColumns;
	}
#endif
}

Z_INLINE void idMatX::transposeMultiply( idMatX &dst, const idMatX &a ) const {
#ifdef MATX_SIMD
	SIMDProcessor->MatX_transposeMultiplyMatX( dst, *this, a );
#else
	int i, j, k, l, n;
	float *dstPtr;
	const float *m1Ptr, *m2Ptr;
	double sum;

	assert( numRows == a.numRows );

	dstPtr = dst.toFloatPtr();
	m1Ptr = toFloatPtr();
	k = numColumns;
	l = a.numColumns;

	for ( i = 0; i < k; i++ ) {
		for ( j = 0; j < l; j++ ) {
			m1Ptr = toFloatPtr() + i;
			m2Ptr = a.toFloatPtr() + j;
			sum = m1Ptr[0] * m2Ptr[0];
			for ( n = 1; n < numRows; n++ ) {
				m1Ptr += numColumns;
				m2Ptr += a.numColumns;
				sum += m1Ptr[0] * m2Ptr[0];
			}
			*dstPtr++ = sum;
		}
	}
#endif
}

Z_INLINE int idMatX::getDimension( void ) const {
	return numRows * numColumns;
}

Z_INLINE const CVec6 &idMatX::SubVec6( int row ) const {
	assert( numColumns >= 6 && row >= 0 && row < numRows );
	return *reinterpret_cast<const CVec6 *>(mat + row * numColumns);
}

Z_INLINE CVec6 &idMatX::SubVec6( int row ) {
	assert( numColumns >= 6 && row >= 0 && row < numRows );
	return *reinterpret_cast<CVec6 *>(mat + row * numColumns);
}

Z_INLINE const idVecX idMatX::SubVecX( int row ) const {
	idVecX v;
	assert( row >= 0 && row < numRows );
	v.SetData( numColumns, mat + row * numColumns );
	return v;
}

Z_INLINE idVecX idMatX::SubVecX( int row ) {
	idVecX v;
	assert( row >= 0 && row < numRows );
	v.SetData( numColumns, mat + row * numColumns );
	return v;
}

Z_INLINE const float *idMatX::toFloatPtr( void ) const {
	return mat;
}

Z_INLINE float *idMatX::toFloatPtr( void ) {
	return mat;
}
//*/

#endif /* MATRIX_H */
